From f63121d2278adc876ab0cd7df6f5f810bc81e01b Mon Sep 17 00:00:00 2001
From: beuchatp <beuchatp@control.ee.ethz.ch>
Date: Wed, 13 Feb 2019 13:00:43 +0100
Subject: [PATCH] Finished implementing the Default controller (except for the
integrator). Needs compiling and testing. (Also changed the ClientConfig.yaml
to be FlyingAgentClientConfig.yaml)
---
.../GUI_Qt/flyingAgentGUI/forms/mainwindow.ui | 6 +-
.../flyingAgentGUI/include/mainwindow.h | 2 +-
.../GUI_Qt/flyingAgentGUI/src/mainwindow.cpp | 4 +-
.../nodes/DefaultControllerConstants.h | 4 +-
.../include/nodes/DefaultControllerService.h | 127 +++-
.../include/nodes/FlyingAgentClient.h | 22 +-
dfall_ws/src/dfall_pkg/launch/agent.launch | 4 +-
.../src/dfall_pkg/launch/coordinator.launch | 2 +-
.../dfall_pkg/param/DefaultController.yaml | 87 ++-
...nfig.yaml => FlyingAgentClientConfig.yaml} | 7 +-
.../src/dfall_pkg/param/YamlFileNames.yaml | 2 +-
.../src/nodes/DefaultControllerService.cpp | 672 ++++++++++++++++--
.../dfall_pkg/src/nodes/FlyingAgentClient.cpp | 364 +++++-----
13 files changed, 977 insertions(+), 326 deletions(-)
rename dfall_ws/src/dfall_pkg/param/{ClientConfig.yaml => FlyingAgentClientConfig.yaml} (85%)
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 bd79177a..e848317a 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
@@ -204,7 +204,7 @@
<string>LoadYAML</string>
</property>
<addaction name="action_LoadYAML_BatteryMonitor"/>
- <addaction name="action_LoadYAML_ClientConfig"/>
+ <addaction name="action_LoadYAML_FlyingAgentClientConfig"/>
</widget>
<widget class="QMenu" name="menuControllers">
<property name="title">
@@ -239,9 +239,9 @@
<string>BatteryMonitor</string>
</property>
</action>
- <action name="action_LoadYAML_ClientConfig">
+ <action name="action_LoadYAML_FlyingAgentClientConfig">
<property name="text">
- <string>ClientConfig</string>
+ <string>FlyingAgentClientConfig</string>
</property>
</action>
<action name="action_showHideController_default">
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 2cfdc268..916869bd 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
@@ -124,7 +124,7 @@ private slots:
// PRIVATE METHODS FOR MENU ITEM CALLBACKS
void on_actionShowHide_Coordinator_triggered();
void on_action_LoadYAML_BatteryMonitor_triggered();
- void on_action_LoadYAML_ClientConfig_triggered();
+ void on_action_LoadYAML_FlyingAgentClientConfig_triggered();
// FOR THE CONTROLLERS MENU
void on_action_showHideController_default_changed();
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 239aa269..0a523fe3 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
@@ -194,7 +194,7 @@ void MainWindow::on_action_LoadYAML_BatteryMonitor_triggered()
}
-void MainWindow::on_action_LoadYAML_ClientConfig_triggered()
+void MainWindow::on_action_LoadYAML_FlyingAgentClientConfig_triggered()
{
#ifdef CATKIN_MAKE
// Inform the user that the menu item was selected
@@ -203,7 +203,7 @@ void MainWindow::on_action_LoadYAML_ClientConfig_triggered()
// Create a local variable for the message
StringWithHeader yaml_filename_msg;
// Specify the data
- yaml_filename_msg.data = "ClientConfig";
+ yaml_filename_msg.data = "FlyingAgentClientConfig";
// Set for whom this applies to
yaml_filename_msg.shouldCheckForAgentID = false;
// Send the message
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h
index e32c39e2..a9beaf7e 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h
@@ -41,13 +41,15 @@
// TO IDENITFY THE STATE OF THE DEFAULT CONTROLLER
-#define DEFAULT_CONTROLLER_STATE_UNKNOWN -1
+#define DEFAULT_CONTROLLER_STATE_NORMAL 1
#define DEFAULT_CONTROLLER_STATE_STANDBY 99
+#define DEFAULT_CONTROLLER_STATE_UNKNOWN -1
// > The sequence of states for a TAKE-OFF manoeuvre
#define DEFAULT_CONTROLLER_STATE_TAKEOFF_SPIN_MOTORS 11
#define DEFAULT_CONTROLLER_STATE_TAKEOFF_MOVE_UP 12
#define DEFAULT_CONTROLLER_STATE_TAKEOFF_GOTO_SETPOINT 13
+#define DEFAULT_CONTROLLER_STATE_TAKEOFF_INTEGRATOR_ON 14
// > The sequence of states for a LANDING manoeuvre
#define DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN 21
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index 8900c471..1fc29bdb 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -104,10 +104,28 @@ using namespace dfall_pkg;
// NOTE: many constants are already defined in the "Constant.h" header file
-// These constants define the method used for estimating the Inertial
-// frame state.
-// All methods are run at all times, this flag indicates which estimate
-// is passed onto the controller.
+// These constants define the method used for computing
+// the control actions from the state error estimates.
+// The following is a short description about each mode:
+//
+// CONTROLLER_METHOD_RATES
+// Uses the poisition, linear velocity and angle
+// error estimates to compute the rates
+//
+// CONTROLLER_METHOD_RATE_ANGLE_NESTED
+// Uses the position and linear velocity error
+// estimates to compute an angle, and then uses
+// this as a reference to construct an angle error
+// estimate and compute from that the rates
+//
+#define CONTROLLER_METHOD_RATES 1
+#define CONTROLLER_METHOD_RATE_ANGLE_NESTED 2 // (DEFAULT)
+
+
+// These constants define the method used for estimating
+// the Inertial frame state.
+// All methods are run at all times, this flag indicates
+// which estimate is passed onto the controller.
// The following is a short description about each mode:
//
// ESTIMATOR_METHOD_FINITE_DIFFERENCE
@@ -160,21 +178,58 @@ float m_time_in_seconds = 0.0;
float yaml_max_setpoint_change_per_second_horizontal = 0.1;
float yaml_max_setpoint_change_per_second_vertical = 0.1;
+// Max error for z
+float yaml_max_setpoint_error_z = 0.4;
+
// Max error for yaw angle
float yaml_max_setpoint_error_yaw_degrees = 60.0;
float yaml_max_setpoint_error_yaw_radians = 60.0 * DEG2RAD;
+// Max {roll,pitch} angle request
+float yaml_max_roll_pitch_request_degrees = 30.0;
+float yaml_max_roll_pitch_request_radians = 30.0 * DEG2RAD;
+
+// Theshold for {roll,pitch} angle beyond
+// which the motors are turned off
+float yaml_threshold_roll_pitch_for_turn_off_degrees = 70.0;
+float yaml_threshold_roll_pitch_for_turn_off_radians = 70.0 * DEG2RAD;
+
// The thrust for take off spin motors
float yaml_takeoff_spin_motors_thrust = 8000;
-// The time for the take off spin(-up) motors
-float takoff_spin_motots_time = 0.8;
+// The time for: take off spin(-up) motors
+float yaml_takoff_spin_motors_time = 0.8;
// Height change for the take off move-up
float yaml_takeoff_move_up_start_height = 0.1;
float yaml_takeoff_move_up_end_height = 0.4;
-// The time for the take off spin motors
+// The time for: take off spin motors
float yaml_takoff_move_up_time = 1.2;
+// Minimum and maximum allowed time for: take off goto setpoint
+float yaml_takoff_goto_setpoint_min_time = 1.2;
+float yaml_takoff_goto_setpoint_max_time = 2.0;
+
+// Box within which to keep the integrator on
+// > Units of [meters]
+// > The box consider is plus/minus this value
+float yaml_takoff_integrator_on_box_horizontal = 0.25;
+float yaml_takoff_integrator_on_box_vertical = 0.15;
+// The time for: take off integrator-on
+float yaml_takoff_integrator_on_time = 1.5;
+
+
+// Height change for the landing move-down
+float yaml_landing_move_down_end_height_setpoint = 0.05;
+float yaml_landing_move_down_end_height_threshold = 0.10;
+// The time for: landing move-down
+float yaml_landing_move_down_time_max = 2.0;
+
+// The thrust for landing spin motors
+float yaml_landing_spin_motors_thrust = 10000;
+// The time for: landing spin motors
+float yaml_landing_spin_motors_time = 1.0;
+
+
// The setpoint to be tracked, the ordering is (x,y,z,yaw),
@@ -244,12 +299,21 @@ bool yaml_shouldDisplayDebugInfo = false;
// VARIABLES FOR THE CONTROLLER
-// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
-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};
+// > A flag for which controller to use:
+int yaml_controller_method = CONTROLLER_METHOD_RATE_ANGLE_NESTED;
+// The LQR Controller parameters for z-height
+std::vector<float> yaml_gainMatrixThrust_2StateVector = { 0.98, 0.25};
+// The LQR Controller parameters for "CONTROLLER_METHOD_RATES"
+std::vector<float> yaml_gainMatrixRollRate_3StateVector = {-6.20,-3.00, 5.20};
+std::vector<float> yaml_gainMatrixPitchRate_3StateVector = { 6.20, 3.00, 5.20};
+// The LQR Controller parameters for "CONTROLLER_METHOD_RATE_ANGLE_NESTED"
+std::vector<float> yaml_gainMatrixRollAngle_2StateVector = {-0.20,-0.20};
+std::vector<float> yaml_gainMatrixPitchAngle_2StateVector = { 0.20, 0.20};
+float yaml_gainRollRate_fromAngle = 4.00;
+float yaml_gainPitchRate_fromAngle = 4.00;
+// The LQR Controller parameters for yaw
+float yaml_gainYawRate_fromAngle = 2.30;
// VARIABLES FOR THE ESTIMATOR
@@ -259,9 +323,10 @@ float m_estimator_frequency = 200.0;
// > A flag for which estimator to use:
int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
+
// > The current state interial estimate,
// for use by the controller
-float m_current_stateInertialEstimate[12];
+float m_current_stateInertialEstimate[9];
// > The measurement of the Crazyflie at the "current" time step,
// to avoid confusion
@@ -273,22 +338,17 @@ float m_previous_xzy_rpy_measurement[6];
// > The full 12 state estimate maintained by the finite
// difference state estimator
-float m_stateInterialEstimate_viaFiniteDifference[12];
+float m_stateInterialEstimate_viaFiniteDifference[9];
// > The full 12 state estimate maintained by the point mass
// kalman filter state estimator
-float m_stateInterialEstimate_viaPointMassKalmanFilter[12];
+float m_stateInterialEstimate_viaPointMassKalmanFilter[9];
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
std::vector<float> yaml_PMKF_Ahat_row1_for_positions = { 0.6723, 0.0034};
std::vector<float> yaml_PMKF_Ahat_row2_for_positions = {-12.9648, 0.9352};
std::vector<float> yaml_PMKF_Kinf_for_positions = { 0.3277,12.9648};
-// > For the (roll,pitch,yaw) angles
-std::vector<float> yaml_PMKF_Ahat_row1_for_angles = { 0.6954, 0.0035};
-std::vector<float> yaml_PMKF_Ahat_row2_for_angles = {-11.0342, 0.9448};
-std::vector<float> yaml_PMKF_Kinf_for_angles = { 0.3046,11.0342};
-
// VARIABLES RELATING TO PUBLISHING
@@ -301,6 +361,11 @@ ros::Publisher m_debugPublisher;
ros::Publisher m_setpointChangedPublisher;
+// ROS Publisher for sending motors-off command
+// to the flying agent client
+ros::Publisher m_motorsOffToFlyingAgentClientPublisher;
+
+
@@ -335,11 +400,28 @@ bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Re
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+// > For the normal state
+void computeResponse_for_normal(Controller::Response &response);
+// > For the standby state (also used for unknown state)
+void computeResponse_for_standby(Controller::Response &response);
+// > For the take-off phases
+void computeResponse_for_takeoff_move_up(Controller::Response &response);
+void computeResponse_for_takeoff_spin_motors(Controller::Response &response);
+void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response);
+void computeResponse_for_takeoff_integrator_on(Controller::Response &response);
+// > For the landing phases
+void computeResponse_for_landing_move_down(Controller::Response &response);
+void computeResponse_for_landing_spin_motors(Controller::Response &response);
+
+
+// SMOOTHING SETPOINT CHANGES
+void smoothSetpointChanges( float target_setpoint[4] , float (¤t_setpoint)[4] );
+
// > This function constructs the error in the body frame
// before calling the appropriate control function
void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateInertial[12], Controller::Response &response)
// > The various functions that implement an LQR controller
-void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller::Response &response);
+void calculateControlOutput_viaLQR_givenError(float stateErrorBody[12], Controller::Response &response);
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
@@ -371,6 +453,9 @@ void publishCurrentSetpointAndState();
// 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();
+
// LOADING OF YAML PARAMETERS
void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg);
void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
index f315d7de..215f13f2 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
@@ -150,7 +150,7 @@ float yaml_mocap_timeout_duration = 1.0;
bool yaml_isEnabled_strictSafety = true;
// > The maximum allowed tilt angle, in degrees and radians
float yaml_maxTiltAngle_for_strictSafety_degrees = 70;
-float m_maxTiltAngle_for_strictSafety_radians = 70 * DEG2RAD;
+float yaml_maxTiltAngle_for_strictSafety_radians = 70 * DEG2RAD;
@@ -192,15 +192,15 @@ int m_controller_nominally_selected;
// VARIABLES FOR THE CONTROLER SERVIVCE CLIENTS
-// The default controller specified in the ClientConfig.yaml
+// The default controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_defaultController;
-// The Student controller specified in the ClientConfig.yaml
+// The Student controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_studentController;
-// The Tuning controller specified in the ClientConfig.yaml
+// The Tuning controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_tuningController;
-// The Picker controller specified in the ClientConfig.yaml
+// The Picker controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_pickerController;
-// The Template controller specified in the ClientConfig.yaml
+// The Template controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_templateController;
@@ -216,7 +216,7 @@ int m_crazyradio_status;
CrazyflieContext m_context;
// Service Client from which the "CrazyflieContext" is loaded
-ros::ServiceClient centralManager;
+ros::ServiceClient m_centralManager;
// Publisher for the control actions to be sent on
// to the Crazyflie (the CrazyRadio node listen to this
@@ -232,10 +232,10 @@ ros::Publisher m_flyingStatePublisher;
//ros::Publisher commandPublisher;
// Publisher Communication with crazyRadio node. Connect and disconnect
-ros::Publisher crazyRadioCommandPublisher;
+ros::Publisher m_crazyRadioCommandPublisher;
// Publisher for which controller is currently being used
-ros::Publisher controllerUsedPublisher;
+ros::Publisher m_controllerUsedPublisher;
@@ -358,5 +358,5 @@ void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
// FOR LOADING THE YAML PARAMETERS
-void isReadyClientConfigYamlCallback(const IntWithHeader & msg);
-void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
+void isReadyFlyingAgentClientConfigYamlCallback(const IntWithHeader & msg);
+void fetchFlyingAgentClientConfigYamlParameters(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 b97eae9a..52a40b94 100755
--- a/dfall_ws/src/dfall_pkg/launch/agent.launch
+++ b/dfall_ws/src/dfall_pkg/launch/agent.launch
@@ -130,8 +130,8 @@
<param name="agentID" value="$(arg agentID)" />
<rosparam
command = "load"
- file = "$(find dfall_pkg)/param/ClientConfig.yaml"
- ns = "ClientConfig"
+ file = "$(find dfall_pkg)/param/FlyingAgentClientConfig.yaml"
+ ns = "FlyingAgentClientConfig"
/>
<rosparam
command = "load"
diff --git a/dfall_ws/src/dfall_pkg/launch/coordinator.launch b/dfall_ws/src/dfall_pkg/launch/coordinator.launch
index 9d10deaf..c9e7e154 100755
--- a/dfall_ws/src/dfall_pkg/launch/coordinator.launch
+++ b/dfall_ws/src/dfall_pkg/launch/coordinator.launch
@@ -44,7 +44,7 @@
/>
<rosparam
command = "load"
- file = "$(find dfall_pkg)/param/ClientConfig.yaml"
+ file = "$(find dfall_pkg)/param/FlyingAgentClientConfig.yaml"
ns = "SafeController"
/>
</node>
diff --git a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
index d64bc24b..d5b432c5 100644
--- a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
@@ -2,23 +2,62 @@
# PARAMTERS FOR THE TAKE-OFF AND LANDING MANOEUVRES
# Max setpoint change per second
-max_setpoint_change_per_second_horizontal : 0.20 # [meters]
-max_setpoint_change_per_second_vertical : 0.10 # [meters]
+max_setpoint_change_per_second_horizontal : 0.50 # [meters]
+max_setpoint_change_per_second_vertical : 0.30 # [meters]
+
+# Max error for z
+max_setpoint_error_z: 0.4
# Max error for yaw angle
max_setpoint_error_yaw_degrees: 60
+# Max {roll,pitch} angle request
+max_roll_pitch_request_degrees: 30
+
+# Theshold for {roll,pitch} angle beyond
+# which the motors are turned off
+threshold_roll_pitch_for_turn_off_degrees: 70
+
# The thrust for take off spin motors
takeoff_spin_motors_thrust: 8000
-# The time for the take off spin motors
-takoff_spin_motots_time: 0.8
+# The time for: take off spin motors
+takoff_spin_motors_time: 0.8
# Height change for the take off move-up
takeoff_move_up_start_height: 0.1
takeoff_move_up_end_height: 0.4
-# The time for the take off spin motors
+# The time for: take off move-up
takoff_move_up_time: 1.2
+# Minimum and maximum allowed time for: take off goto setpoint
+takoff_goto_setpoint_min_time: 1.0
+takoff_goto_setpoint_max_time: 2.0
+
+# Box within which to keep the integrator on
+# > Units of [meters]
+# > The box consider is plus/minus this value
+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: 1.5
+
+
+# Height change for the landing move-down
+landing_move_down_end_height_setpoint: 0.05
+landing_move_down_end_height_threshold: 0.10
+# The time for: landing move-down
+landing_move_down_time_max: 2.0
+
+# The thrust for landing spin motors
+landing_spin_motors_thrust: 10000
+# The time for: landing spin motors
+landing_spin_motors_time: 1.0
+
+
+# IMPORTANT NOTE: the times above should NOT be set
+# to zero because this will cause a divide by zero
+# crash.
+
# ------------------------------------------------------
# PARAMTERS THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
@@ -46,11 +85,25 @@ shouldPublishDebugMessage : false
# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
shouldDisplayDebugInfo : false
-# The LQR Controller parameters for "mode = 3"
-gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
-gainMatrixRollRate : [ 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00]
-gainMatrixPitchRate : [ 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00]
-gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
+# A flag for which controller to use, defined as:
+# 1 - Rate controller
+# 2 - Angle-Rate nested controller
+controller_method : 1
+
+# The LQR Controller parameters for z-height
+gainMatrixThrust_2StateVector : [ 0.98, 0.25]
+# The LQR Controller parameters for mode 1 (the Rate controller)
+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]
+gainRollRate_fromAngle : 4.00
+gainPitchRate_fromAngle : 4.00
+# The LQR Controller parameters for yaw
+gainYawRate_fromAngle : 2.30
+
+
# A flag for which estimator to use, defined as:
# 1 - Finite Different Method,
@@ -60,22 +113,10 @@ gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0
# 2 - Point Mass Per Dimension Method
# Uses a 2nd order random walk estimator independently for
# each of (x,y,z,roll,pitch,yaw)
-# 3 - Quad-rotor Model Based Method
-# Uses the model of the quad-rotor and the previous inputs
estimator_method : 1
# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
# > For the (x,y,z) position
PMKF_Ahat_row1_for_positions : [ 0.6723, 0.0034]
PMKF_Ahat_row2_for_positions : [-12.9648, 0.9352]
-PMKF_Kinf_for_positions : [ 0.3277,12.9648]
-# > For the (roll,pitch,yaw) angles
-PMKF_Ahat_row1_for_angles : [ 0.6954, 0.0035]
-PMKF_Ahat_row2_for_angles : [-11.0342, 0.9448]
-PMKF_Kinf_for_angles : [ 0.3046,11.0342]
-
-#PMKF_Ahat_row1_for_angles : [ 0.6723, 0.0034]
-#PMKF_Ahat_row2_for_angles : [-12.9648, 0.9352]
-#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
-
-
+PMKF_Kinf_for_positions : [ 0.3277,12.9648]
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/param/ClientConfig.yaml b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
similarity index 85%
rename from dfall_ws/src/dfall_pkg/param/ClientConfig.yaml
rename to dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
index b6d75db6..cf56b8e1 100755
--- a/dfall_ws/src/dfall_pkg/param/ClientConfig.yaml
+++ b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
@@ -28,9 +28,4 @@ shouldPerfom_takeOff_with_defaultController: true
# Flag for whether the landing should be performed
# with the default controller
-shouldPerfom_landing_with_defaultController: true
-
-
-#battery_threshold_while_flying: 2.60 # in V
-#battery_threshold_while_motors_off: 3.30 # in V
-#battery_polling_period: 200 # in ms
\ No newline at end of file
+shouldPerfom_landing_with_defaultController: true
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml b/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
index 53fd75c2..b61a3e84 100644
--- a/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
+++ b/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
@@ -1,4 +1,4 @@
dictionary : {
- 'ClientConfig' : 'ClientConfig' ,
+ 'FlyingAgentClientConfig' : 'FlyingAgentClientConfig' ,
'SafeController' : 'SafeController'
}
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index 96711b81..c5e0188e 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -92,7 +92,13 @@ bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Re
m_current_state = DEFAULT_CONTROLLER_STATE_TAKE_OFF_SPIN_MOTORS;
m_current_state_changed = true;
// Fill in the response duration in milliseconds
- response.data = 3000;
+ response.data = 1000 *
+ int(
+ + yaml_takoff_spin_motors_time
+ + yaml_takoff_move_up_time
+ + yaml_takoff_goto_setpoint_max_time
+ + yaml_takoff_integrator_on_time
+ );
break;
}
@@ -106,7 +112,11 @@ bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Re
m_current_state = DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN;
m_current_state_changed = true;
// Fill in the response duration in milliseconds
- response.data = 2000;
+ response.data = 1000 *
+ int(
+ + yaml_landing_move_down_time_max
+ + yaml_landing_spin_motors_time
+ );
break;
}
@@ -175,6 +185,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Switch between the possible states
switch (m_current_state)
{
+ case DEFAULT_CONTROLLER_STATE_NORMAL:
+ computeResponse_for_normal(response);
+ break;
+
case DEFAULT_CONTROLLER_STATE_TAKE_OFF_SPIN_MOTORS:
computeResponse_for_takeoff_spin_motors(response);
break;
@@ -183,13 +197,57 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
computeResponse_for_takeoff_move_up(response);
break;
+ case DEFAULT_CONTROLLER_STATE_TAKEOFF_GOTO_SETPOINT:
+ computeResponse_for_takeoff_goto_setpoint(response);
+ break;
+
+ case DEFAULT_CONTROLLER_STATE_TAKEOFF_INTEGRATOR_ON:
+ computeResponse_for_takeoff_integrator_on(response);
+ break;
+
+ case DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN:
+ computeResponse_for_landing_move_down(response);
+ break;
+
+ case DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS:
+ computeResponse_for_landing_spin_motors(response);
+ break;
+
+ case DEFAULT_CONTROLLER_STATE_STANDBY:
+ case DEFAULT_CONTROLLER_STATE_UNKNOWN:
+ default:
+ computeResponse_for_standby(response);
+ break;
}
-
- // CARRY OUT THE CONTROLLER COMPUTATIONS
- // Call the function that performs the control computations for this mode
- calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response);
+ // Change to standby state if the {roll,pitch}
+ // angles exceed the threshold
+ if (
+ (abs(m_current_stateInertialEstimate[6]) > yaml_threshold_roll_pitch_for_turn_off_radians)
+ or
+ (abs(m_current_stateInertialEstimate[7]) > yaml_threshold_roll_pitch_for_turn_off_radians)
+ )
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Angle thereshold exceeded. 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 command to the "Flying Agent Client"
+ // requesting the "MOTORS-OFF" state
+ publish_motors_off_to_flying_agent_client();
+ }
+
+
+ // If the state changed,
+ // then publish the setpoint so that the GUI is updated
+ if (m_current_state_changed)
+ {
+ publishCurrentSetpointAndState();
+ }
// PUBLISH THE DEBUG MESSAGE (if required)
@@ -198,11 +256,62 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
construct_and_publish_debug_message(request,response);
}
+
// Return "true" to indicate that the control computation was performed successfully
return true;
}
+void computeResponse_for_standby(Controller::Response &response)
+{
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Nothing to perform for this state
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ }
+
+ // Fill in zero for the angle parts
+ response.controlOutput.roll = 0.0;
+ response.controlOutput.pitch = 0.0;
+ response.controlOutput.yaw = 0.0;
+
+ // Fill in all motor thrusts as zero
+ response.controlOutput.motorCmd1 = 0.0;
+ response.controlOutput.motorCmd2 = 0.0;
+ response.controlOutput.motorCmd3 = 0.0;
+ response.controlOutput.motorCmd4 = 0.0;
+
+ // Specify that using a "motor type" of command
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+}
+
+
+void computeResponse_for_normal(Controller::Response &response)
+{
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Set the "m_setpoint_for_controller" variable
+ // to the current inertial estimate
+ m_setpoint_for_controller[0] = m_current_stateInertialEstimate[0];
+ m_setpoint_for_controller[1] = m_current_stateInertialEstimate[1];
+ m_setpoint_for_controller[2] = m_current_stateInertialEstimate[2];
+ m_setpoint_for_controller[3] = m_current_stateInertialEstimate[8];
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ }
+
+ // Smooth out any setpoint changes
+ smoothSetpointChanges( m_setpoint , m_setpoint_for_controller );
+
+ // Call the LQR control function
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response);
+
+}
void computeResponse_for_takeoff_spin_motors(Controller::Response &response)
@@ -216,12 +325,15 @@ void computeResponse_for_takeoff_spin_motors(Controller::Response &response)
m_current_state_changed = false;
}
+ // Compute the time elapsed as a proportion
+ float time_elapsed_proportion = m_time_in_seconds / yaml_takoff_spin_motors_time;
+ if (time_elapsed_proportion > 1.0)
+ time_elapsed_proportion = 1.0;
+
// Compute the "spinning" thrust
- float thrust_for_spinning = 1000.0;
- if (m_time_in_seconds < takoff_spin_motots_time)
- thrust_for_spinning += yaml_takeoff_spin_motors_thrust * (m_time_in_seconds/takoff_spin_motots_time);
- else
- thrust_for_spinning += yaml_takeoff_spin_motors_thrust;
+ float thrust_for_spinning =
+ + 1000.0
+ + time_elapsed_proportion * yaml_takeoff_spin_motors_thrust;
// Fill in zero for the angle parts
response.controlOutput.roll = 0.0;
@@ -238,7 +350,7 @@ void computeResponse_for_takeoff_spin_motors(Controller::Response &response)
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
// Change to next state after specified time
- if (m_time_in_seconds > takoff_spin_motots_time)
+ if (m_time_in_seconds > yaml_takoff_spin_motors_time)
{
// Inform the user
ROS_INFO("[DEFAULT CONTROLLER] Switch to state: take-off move up");
@@ -290,14 +402,292 @@ void computeResponse_for_takeoff_move_up(Controller::Response &response)
m_setpoint_for_controller[3] = initial_yaw + time_elapsed_proportion * yaw_start_to_end_diff;
// Call the LQR control function
- calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response)
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response);
+ // Change to next state after specified time
+ if (m_time_in_seconds > yaml_takoff_move_up_time)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Switch to state: take-off goto setpoint");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_TAKEOFF_GOTO_SETPOINT;
+ m_current_state_changed = true;
+ }
}
+void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response)
+{
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Nothing to perform for this state
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ }
+
+ // Smooth out any setpoint changes
+ smoothSetpointChanges( m_setpoint , m_setpoint_for_controller );
+
+ // Call the LQR control function
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response);
+
+ // If minimum time is passed,
+ // then check if near to the setpoint
+ if (m_time_in_seconds > yaml_takoff_goto_setpoint_min_time)
+ {
+ // Compute the current errors
+ float error_x = m_setpoint[0] - m_current_stateInertialEstimate[0];
+ float error_y = m_setpoint[1] - m_current_stateInertialEstimate[1];
+ float error_z = m_setpoint[2] - m_current_stateInertialEstimate[2];
+ // Check if within the "integrator on" box
+ // of the setpoint
+ if (
+ (abs(error_x) < yaml_takoff_integrator_on_box_horizontal)
+ and
+ (abs(error_y) < yaml_takoff_integrator_on_box_horizontal)
+ and
+ (abs(error_z) < yaml_takoff_integrator_on_box_vertical)
+ )
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Switch to state: take-off integrator on");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_TAKEOFF_INTEGRATOR_ON;
+ m_current_state_changed = true;
+ }
+
+ // Change to normal if the timeout is reched
+ if (m_time_in_seconds > yaml_takoff_goto_setpoint_max_time)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Did not reached the setpoint within the \"take-off goto setpoint\" allowed time. 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;
+ }
+}
+
+
+void computeResponse_for_integrator_on(Controller::Response &response)
+{
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Set the "m_setpoint_for_controller" variable
+ // to the current setpoint
+ m_setpoint_for_controller[0] = m_setpoint[0];
+ m_setpoint_for_controller[1] = m_setpoint[1];
+ m_setpoint_for_controller[2] = m_setpoint[2];
+ m_setpoint_for_controller[3] = m_setpoint[3];
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ }
+ // Call the LQR control function
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response);
+
+ // Change to next state after specified time
+ if (m_time_in_seconds > yaml_takoff_integrator_on_time)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] 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;
+ }
+}
+
+
+
+
+
+void computeResponse_for_landing_move_down(Controller::Response &response)
+{
+ // Initialise a static variable for the starting height and yaw
+ static float initial_height = 0.4;
+
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Set the current (x,y,yaw) location as the setpoint
+ m_setpoint_for_controller[0] = m_current_stateInertialEstimate[0];
+ m_setpoint_for_controller[1] = m_current_stateInertialEstimate[1];
+ m_setpoint_for_controller[3] = m_current_stateInertialEstimate[8];
+ // Store the current z
+ initial_height = m_current_stateInertialEstimate[2];
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ }
+
+ // Compute the time elapsed as a proportion
+ float time_elapsed_proportion = m_time_in_seconds / (0.8*yaml_landing_move_down_time_max);
+ if (time_elapsed_proportion > 1.0)
+ time_elapsed_proportion = 1.0;
+
+ // Compute the z-height setpoint
+ m_setpoint_for_controller[2] = initial_height + time_elapsed_proportion * (yaml_landing_move_down_end_height_setpoint-initial_height);
+
+ // Call the LQR control function
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response);
+
+ // Check if within the threshold of zero
+ if (m_current_stateInertialEstimate[2] < yaml_landing_move_down_end_height_threshold)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Switch to state: landing spin motors");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS;
+ m_current_state_changed = true;
+ }
+
+ // Change to landing spin motors if the timeout is reached
+ if (m_time_in_seconds > yaml_landing_move_down_time_max)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Did not reached the setpoint within the \"landing move down\" allowed time. Switch to state: landing spin motors");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS;
+ m_current_state_changed = true;
+ }
+}
+
+
+void computeResponse_for_landing_spin_motors(Controller::Response &response)
+{
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Set the z setpoint
+ m_setpoint_for_controller[2] = yaml_landing_move_down_end_height_setpoint;
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ }
+
+ // Compute the time elapsed as a proportion
+ float time_elapsed_proportion = m_time_in_seconds / yaml_landing_spin_motors_time;
+ if (time_elapsed_proportion > 1.0)
+ time_elapsed_proportion = 1.0;
+
+
+ // Start by using the controller and reducing the thrust
+ if (time_elapsed_proportion<0.5)
+ {
+ // Call the LQR control function
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response);
+ // Compute the desired "spinning" thrust
+ float thrust_for_spinning =
+ (1.0-time_elapsed_proportion)
+ *
+ computeMotorPolyBackward(m_cf_weight_in_newtons/4.0);
+ // Adjust the motor commands
+ response.controlOutput.motorCmd1 = thrust_for_spinning;
+ response.controlOutput.motorCmd2 = thrust_for_spinning;
+ response.controlOutput.motorCmd3 = thrust_for_spinning;
+ response.controlOutput.motorCmd4 = thrust_for_spinning;
+ }
+ // Next stop using the controller and just spin the motors
+ else
+ {
+ // Fill in zero for the angle parts
+ response.controlOutput.roll = 0.0;
+ response.controlOutput.pitch = 0.0;
+ response.controlOutput.yaw = 0.0;
+ // Fill in all motor thrusts as the same
+ response.controlOutput.motorCmd1 = yaml_landing_spin_motors_thrust;
+ response.controlOutput.motorCmd2 = yaml_landing_spin_motors_thrust;
+ response.controlOutput.motorCmd3 = yaml_landing_spin_motors_thrust;
+ response.controlOutput.motorCmd4 = yaml_landing_spin_motors_thrust;
+ // Specify that using a "motor type" of command
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+
+ // Change to next state after specified time
+ if (m_time_in_seconds > yaml_landing_spin_motors_time)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] 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;
+ }
+ }
+}
+
+
+// ------------------------------------------------------------------------------
+// SSSS M M OOO OOO TTTTT H H
+// S MM MM O O O O T H H
+// SSS M M M O O O O T HHHHH
+// S M M O O O O T H H
+// SSSS M M OOO OOO T H H
+//
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+// ------------------------------------------------------------------------------
+
+
+void smoothSetpointChanges( float target_setpoint[4] , float (¤t_setpoint)[4] )
+{
+ // SMOOTH THE Z-COORIDINATE
+ // > Compute the max allowed change
+ 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
+ float max_for_xy = yaml_max_setpoint_change_per_second_horizontal yaml_control_frequency;
+ // > Compute the current difference
+ 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^2 + diff_for_y^2 );
+ // > Clip the difference to the maximum
+ if (diff_for_xy > max_for_xy)
+ {
+ // > Convert the difference to a proportion
+ float proportion_xy = max_for_xy / diff_for_xy;
+ // > Update the current setpoint
+ current_setpoint[0] += proportion_xy * diff_for_x;
+ current_setpoint[1] += proportion_xy * diff_for_y;
+ }
+ else
+ {
+ // > Update the current setpoint to be the
+ // the target setpoint because it is within
+ // reach
+ current_setpoint[0] = target_setpoint[0];
+ current_setpoint[1] = target_setpoint[1];
+ }
+}
@@ -341,7 +731,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
{
// Transfer the estimate
- for(int i = 0; i < 12; ++i)
+ for(int i = 0; i < 9; ++i)
{
m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
}
@@ -351,7 +741,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
{
// Transfer the estimate
- for(int i = 0; i < 12; ++i)
+ for(int i = 0; i < 9; ++i)
{
m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
}
@@ -361,9 +751,9 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
default:
{
// Display that the "yaml_estimator_method" was not recognised
- ROS_INFO_STREAM("[PICKER CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'PickerControllerService': the 'yaml_estimator_method' is not recognised.");
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DefaultControllerService': the 'yaml_estimator_method' is not recognised.");
// Transfer the finite difference estimate by default
- for(int i = 0; i < 12; ++i)
+ for(int i = 0; i < 9; ++i)
{
m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
}
@@ -403,10 +793,6 @@ void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
m_stateInterialEstimate_viaFiniteDifference[3] = (m_current_xzy_rpy_measurement[0] - m_previous_xzy_rpy_measurement[0]) * m_estimator_frequency;
m_stateInterialEstimate_viaFiniteDifference[4] = (m_current_xzy_rpy_measurement[1] - m_previous_xzy_rpy_measurement[1]) * m_estimator_frequency;
m_stateInterialEstimate_viaFiniteDifference[5] = (m_current_xzy_rpy_measurement[2] - m_previous_xzy_rpy_measurement[2]) * m_estimator_frequency;
- // > for (roll,pitch,yaw) velocities
- m_stateInterialEstimate_viaFiniteDifference[9] = (m_current_xzy_rpy_measurement[3] - m_previous_xzy_rpy_measurement[3]) * m_estimator_frequency;
- m_stateInterialEstimate_viaFiniteDifference[10] = (m_current_xzy_rpy_measurement[4] - m_previous_xzy_rpy_measurement[4]) * m_estimator_frequency;
- m_stateInterialEstimate_viaFiniteDifference[11] = (m_current_xzy_rpy_measurement[5] - m_previous_xzy_rpy_measurement[5]) * m_estimator_frequency;
}
@@ -415,8 +801,8 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
{
// PERFORM THE KALMAN FILTER UPDATE STEP
// > First take a copy of the estimator state
- float temp_PMKFstate[12];
- for(int i = 0; i < 12; ++i)
+ float temp_PMKFstate[9];
+ for(int i = 0; i < 9; ++i)
{
temp_PMKFstate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
}
@@ -431,16 +817,12 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
m_stateInterialEstimate_viaPointMassKalmanFilter[2] = yaml_PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + yaml_PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + yaml_PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[2];
m_stateInterialEstimate_viaPointMassKalmanFilter[5] = yaml_PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + yaml_PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + yaml_PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[2];
- // > roll position and velocity:
- m_stateInterialEstimate_viaPointMassKalmanFilter[6] = yaml_PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + yaml_PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + yaml_PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[3];
- m_stateInterialEstimate_viaPointMassKalmanFilter[9] = yaml_PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + yaml_PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + yaml_PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[3];
- // > pitch position and velocity:
- m_stateInterialEstimate_viaPointMassKalmanFilter[7] = yaml_PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + yaml_PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + yaml_PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[4];
- m_stateInterialEstimate_viaPointMassKalmanFilter[10] = yaml_PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + yaml_PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + yaml_PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[4];
- // > yaw position and velocity:
- m_stateInterialEstimate_viaPointMassKalmanFilter[8] = yaml_PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + yaml_PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + yaml_PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[5];
- m_stateInterialEstimate_viaPointMassKalmanFilter[11] = yaml_PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + yaml_PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + yaml_PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[5];
-}
+ // > for (roll,pitch,yaw) angles
+ // (taken directly from the measurement):
+ m_stateInterialEstimate_viaPointMassKalmanFilter[6] = m_current_xzy_rpy_measurement[3];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[7] = m_current_xzy_rpy_measurement[4];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[8] = m_current_xzy_rpy_measurement[5];
+}
@@ -467,14 +849,10 @@ void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateI
stateInertial[2] = stateInertial[2] - setpoint[2];
// Clip the z-coordination to within the specified bounds
- if (stateInertial[2] > 0.40f)
- {
- stateInertial[2] = 0.40f;
- }
- else if (stateInertial[2] < -0.40f)
- {
- stateInertial[2] = -0.40f;
- }
+ if (stateInertial[2] > yaml_max_setpoint_error_z)
+ stateInertial[2] = yaml_max_setpoint_error_z;
+ else if (stateInertial[2] < -yaml_max_setpoint_error_z)
+ stateInertial[2] = -yaml_max_setpoint_error_z;
// Fill in the yaw angle error
// > This error should be "unwrapped" to be in the range
@@ -485,53 +863,110 @@ void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateI
while(yawError > PI) {yawError -= 2 * PI;}
while(yawError < -PI) {yawError += 2 * PI;}
// Clip the error to within the specified bounds
- if (yawError>(PI/3))
- {
- yawError = (PI/3);
- }
- else if (yawError<(-PI/3))
- {
- yawError = (-PI/3);
- }
+ if (yawError > yaml_max_setpoint_error_yaw_radians)
+ yawError = yaml_max_setpoint_error_yaw_radians;
+ else if (yawError < -yaml_max_setpoint_error_yaw_radians)
+ yawError = -yaml_max_setpoint_error_yaw_radians;
+
// > Finally, put the "yawError" into the "stateError" variable
stateInertial[8] = yawError;
// CONVERSION INTO BODY FRAME
- // Convert the state erorr from the Inertial frame into the Body frame
+ // Initialise a variable for the body frame error
+ float bodyFrameError[9];
+ // Call the function to convert the state erorr from
+ // the Inertial frame into the Body frame
convertIntoBodyFrame(stateInertial, bodyFrameError, temp_stateInertial_yaw);
- calculateControlOutput_viaLQRforRates(bodyFrameError, response);
+ calculateControlOutput_viaLQR_givenError(bodyFrameError, response);
}
-void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller::Response &response)
+void calculateControlOutput_viaLQR_givenError(float stateErrorBody[12], Controller::Response &response)
{
// PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+ // Compute the Z-CONTROLLER
+ // > provides the total thrust adjustment
+ float thrustAdjustment =
+ - yaml_gainMatrixThrust_2StateVector[0] * stateErrorBody[2]
+ - yaml_gainMatrixThrust_2StateVector[1] * stateErrorBody[5];
+
+ // Compute the YAW-CONTROLLER
+ // > provides the body frame yaw rate
+ float yawRate_forResponse =
+ - yaml_gainYawRate_fromAngle * stateErrorBody[8];
+
// Instantiate the local variables for the following:
// > body frame roll rate,
// > body frame pitch rate,
- // > body frame yaw rate,
- // > total thrust adjustment.
- // These will be requested from the Crazyflie's on-baord "inner-loop" controller
- float rollRate_forResponse = 0;
- float pitchRate_forResponse = 0;
- float yawRate_forResponse = 0;
- float thrustAdjustment = 0;
-
- // Perform the "-Kx" LQR computation for the rates and thrust:
- for(int i = 0; i < 9; ++i)
+ float rollRate_forResponse = 0.0;
+ float pitchRate_forResponse = 0.0;
+
+ // Switch between the differnt control method for
+ // the X-Y-CONTROLLER
+ switch (yaml_controller_method)
{
- // BODY FRAME Y CONTROLLER
- rollRate_forResponse -= yaml_gainMatrixRollRate[i] * stateErrorBody[i];
- // BODY FRAME X CONTROLLER
- pitchRate_forResponse -= yaml_gainMatrixPitchRate[i] * stateErrorBody[i];
- // BODY FRAME YAW CONTROLLER
- yawRate_forResponse -= yaml_gainMatrixYawRate[i] * stateErrorBody[i];
- // > ALITUDE CONTROLLER (i.e., z-controller):
- thrustAdjustment -= yaml_gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
+ case CONTROLLER_METHOD_RATES:
+ {
+ // Compute the X-CONTROLLER
+ // > provides the body frame pitch rate
+ pitchRate_forResponse =
+ - yaml_gainMatrixPitchRate_3StateVector[0] * stateErrorBody[0]
+ - yaml_gainMatrixPitchRate_3StateVector[1] * stateErrorBody[3]
+ - yaml_gainMatrixPitchRate_3StateVector[2] * stateErrorBody[7];
+
+ // Compute the Y-CONTROLLER
+ // > provides the body frame roll rate
+ rollRate_forResponse =
+ - yaml_gainMatrixRollRate_3StateVector[0] * stateErrorBody[1]
+ - yaml_gainMatrixRollRate_3StateVector[1] * stateErrorBody[4]
+ - yaml_gainMatrixRollRate_3StateVector[2] * stateErrorBody[6];
+ break;
+ }
+
+ case CONTROLLER_METHOD_RATE_ANGLE_NESTED:
+ {
+ // Compute the X-CONTROLLER
+ // > Compute the desired pitch angle
+ float pitchAngle_desired =
+ - yaml_gainMatrixPitchAngle_2StateVector[0] * stateErrorBody[0]
+ - 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;
+ else if (pitchAngle_desired < -yaml_max_roll_pitch_request_radians)
+ pitchAngle_desired = -yaml_max_roll_pitch_request_radians;
+ // > Compute the pitch rate
+ pitchRate_forResponse =
+ - yaml_gainPitchRate_fromAngle * (stateErrorBody[7] - pitchAngle_desired);
+
+ // Compute the Y-CONTROLLER
+ // > Compute the desired roll angle
+ float rollAngle_desired =
+ - yaml_gainMatrixRollAngle_2StateVector[0] * stateErrorBody[1]
+ - yaml_gainMatrixRollAngle_2StateVector[1] * stateErrorBody[4];
+ // Clip the request to within the specified limits
+ if (rollAngle_desired > yaml_max_roll_pitch_request_radians)
+ rollAngle_desired = yaml_max_roll_pitch_request_radians;
+ else if (rollAngle_desired < -yaml_max_roll_pitch_request_radians)
+ rollAngle_desired = -yaml_max_roll_pitch_request_radians;
+ // > Compute the roll rate
+ rollRate_forResponse =
+ - yaml_gainRollRate_fromAngle * (stateErrorBody[6] - rollAngle_desired);
+
+ break;
+ }
+
+ default:
+ {
+ // Inform the user of the error
+ ROS_ERROR("[DEFAULT CONTROLLER] The variable \"yaml_controller_method\" is not recognised.");
+ break;
+ }
}
+
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable
@@ -895,6 +1330,22 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
+// PUBLISH MOTORS-OFF MESSAGE TO FLYING AGENT CLIENT
+void publish_motors_off_to_flying_agent_client()
+{
+ // Instantiate a local variable of type "IntWithHeader"
+ IntWithHeader msg;
+ // Fill in the MOTORS-OFF command
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+ // Fill in the header that this applies
+ msg.shouldCheckForAgentID = false;
+ // Publish the message
+ m_motorsOffToFlyingAgentClientPublisher.publish(msg);
+}
+
+
+
+
// ----------------------------------------------------------------------------------
// L OOO A DDDD
@@ -977,19 +1428,53 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
yaml_max_setpoint_change_per_second_horizontal = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_change_per_second_horizontal");
yaml_max_setpoint_change_per_second_vertical = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_change_per_second_vertical");
+ // Max error for z
+ yaml_max_setpoint_error_z = = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_z");
+
// Max error for yaw angle
yaml_max_setpoint_error_yaw_degrees = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_yaw_degrees");
+ // Max {roll,pitch} angle request
+ yaml_max_roll_pitch_request_degrees = getParameterFloat(nodeHandle_for_paramaters , "max_roll_pitch_request_degrees");
+
+ // Theshold for {roll,pitch} angle beyond
+ // which the motors are turned off
+ yaml_threshold_roll_pitch_for_turn_off_degrees = getParameterFloat(nodeHandle_for_paramaters , "threshold_roll_pitch_for_turn_off_degrees");
+
// The thrust for take off spin motors
yaml_takeoff_spin_motors_thrust = getParameterFloat(nodeHandle_for_paramaters , "takeoff_spin_motors_thrust");
// The time for the take off spin(-up) motors
- yaml_takoff_spin_motots_time = getParameterFloat(nodeHandle_for_paramaters , "takoff_spin_motots_time");
+ yaml_takoff_spin_motors_time = getParameterFloat(nodeHandle_for_paramaters , "takoff_spin_motors_time");
// Height change for the take off move-up
yaml_takeoff_move_up_height = getParameterFloat(nodeHandle_for_paramaters , "takeoff_move_up_height");
// The time for the take off spin motors
yaml_takoff_move_up_time = getParameterFloat(nodeHandle_for_paramaters , "takoff_move_up_time");
+ // Minimum and maximum allowed time for: take off goto setpoint
+ yaml_takoff_goto_setpoint_min_time = getParameterFloat(nodeHandle_for_paramaters , "takoff_goto_setpoint_min_time");
+ yaml_takoff_goto_setpoint_max_time = getParameterFloat(nodeHandle_for_paramaters , "takoff_goto_setpoint_max_time");
+
+ // Box within which to keep the integrator on
+ // > Units of [meters]
+ // > The box consider is plus/minus this value
+ yaml_takoff_integrator_on_box_horizontal = getParameterFloat(nodeHandle_for_paramaters , "takoff_integrator_on_box_horizontal");
+ yaml_takoff_integrator_on_box_vertical = getParameterFloat(nodeHandle_for_paramaters , "takoff_integrator_on_box_vertical");
+ // The time for: take off integrator-on
+ yaml_takoff_integrator_on_time = getParameterFloat(nodeHandle_for_paramaters , "takoff_integrator_on_time");
+
+ // Height change for the landing move-down
+ yaml_landing_move_down_end_height_setpoint = getParameterFloat(nodeHandle_for_paramaters , "landing_move_down_end_height_setpoint");
+ yaml_landing_move_down_end_height_threshold = getParameterFloat(nodeHandle_for_paramaters , "landing_move_down_end_height_threshold");
+ // The time for: landing move-down
+ yaml_landing_move_down_time_max = getParameterFloat(nodeHandle_for_paramaters , "landing_move_down_time_max");
+
+ // The thrust for landing spin motors
+ yaml_landing_spin_motors_thrust = getParameterFloat(nodeHandle_for_paramaters , "landing_spin_motors_thrust");
+ // The time for: landing spin motors
+ yaml_landing_spin_motors_time = getParameterFloat(nodeHandle_for_paramaters , "landing_spin_motors_time");
+
+
// ------------------------------------------------------
@@ -1022,14 +1507,24 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
yaml_shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_paramaters, "shouldDisplayDebugInfo");
- // 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);
+ // > A flag for which controller to use:
+ yaml_controller_method = getParameterInt( nodeHandle_for_paramaters , "controller_method" );
+
+ // The LQR Controller parameters for z-height
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_2StateVector", yaml_gainMatrixThrust_2StateVector, 2);
+ // The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_3StateVector", yaml_gainMatrixRollRate_3StateVector, 3);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_3StateVector", yaml_gainMatrixPitchRate_3StateVector, 3);
+ // The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle_2StateVector", yaml_gainMatrixRollAngle_2StateVector, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle_2StateVector", yaml_gainMatrixPitchAngle_2StateVector, 2);
+ yaml_gainRollRate_fromAngle = getParameterFloat(nodeHandle_for_paramaters, "gainRollRate_fromAngle");
+ yaml_gainPitchRate_fromAngle = getParameterFloat(nodeHandle_for_paramaters, "gainPitchRate_fromAngle");
+ // The LQR Controller parameters for yaw
+ yaml_gainYawRate_fromAngle = getParameterFloat(nodeHandle_for_paramaters, "gainYawRate_fromAngle");
// A flag for which estimator to use:
- yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
+ yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
@@ -1059,6 +1554,16 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > Conver the control frequency to a delta T
m_control_deltaT = 1.0 / yaml_control_frequency;
+ // Max error for yaw angle
+ yaml_max_setpoint_error_yaw_radians = DEG2RAD * yaml_max_setpoint_error_yaw_degrees;
+
+ // Max {roll,pitch} angle request
+ yaml_max_roll_pitch_request_radians = DEG2RAD * yaml_max_roll_pitch_request_degrees;
+
+ // Theshold for {roll,pitch} angle beyond
+ // which the motors are turned off
+ yaml_threshold_roll_pitch_for_turn_off_radians = DEG2RAD * yaml_threshold_roll_pitch_for_turn_off_degrees;
+
// DEBUGGING: Print out one of the computed quantities
ROS_INFO_STREAM("[DEFAULT CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
}
@@ -1281,7 +1786,18 @@ int main(int argc, char* argv[])
// will take to perform (in milliseconds)
ros::ServiceServer requestManoeuvreService = nodeHandle.advertiseService("RequestManoeuvre", requestManoeuvreCallback);
-
+ // Instantiate the class variable "m_motorsOffToFlyingAgentClientPublisher"
+ // to be a "ros::Publisher". This variable advertises under the
+ // name space:
+ // "FlyingAgentClient/Command"
+ // meaning that it is mimicing messages send by the "Fying
+ // Agent Client" node. The message sent has the structure
+ // defined in the file "IntWithHeader.msg".
+ // > First create a node handle to the base namespace
+ // i.e., the namespace: "/dfall/agentXXX/"
+ ros::NodeHandle base_nodeHandle(m_namespace);
+ // > Now instantiate the publisher
+ m_motorsOffToFlyingAgentClientPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/Command", 1);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index 288af49a..d55fd3dc 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -377,9 +377,9 @@ bool safetyCheck_on_positionAndTilt(CrazyflieData data)
{
// Check on the ROLL angle
if(
- (data.roll > m_maxTiltAngle_for_strictSafety_radians)
+ (data.roll > yaml_maxTiltAngle_for_strictSafety_radians)
or
- (data.roll < -m_maxTiltAngle_for_strictSafety_radians)
+ (data.roll < -yaml_maxTiltAngle_for_strictSafety_radians)
)
{
ROS_INFO_STREAM("[FLYING AGENT CLIENT] roll too big.");
@@ -387,9 +387,9 @@ bool safetyCheck_on_positionAndTilt(CrazyflieData data)
}
// Check on the PITCH angle
if(
- (data.pitch > m_maxTiltAngle_for_strictSafety_radians)
+ (data.pitch > yaml_maxTiltAngle_for_strictSafety_radians)
or
- (data.pitch < -m_maxTiltAngle_for_strictSafety_radians)
+ (data.pitch < -yaml_maxTiltAngle_for_strictSafety_radians)
)
{
ROS_INFO_STREAM("[FLYING AGENT CLIENT] pitch too big.");
@@ -721,7 +721,7 @@ void setInstantController(int controller)
// GUI" about this update to the instant controller
std_msgs::Int32 controller_used_msg;
controller_used_msg.data = controller;
- controllerUsedPublisher.publish(controller_used_msg);
+ m_controllerUsedPublisher.publish(controller_used_msg);
}
@@ -1006,9 +1006,9 @@ void loadCrazyflieContext()
CrazyflieContext new_context;
- centralManager.waitForExistence(ros::Duration(-1));
+ m_centralManager.waitForExistence(ros::Duration(-1));
- if(centralManager.call(contextCall)) {
+ if(m_centralManager.call(contextCall)) {
new_context = contextCall.response.crazyflieContext;
ROS_INFO_STREAM("[FLYING AGENT CLIENT] CrazyflieContext:\n" << new_context);
@@ -1027,7 +1027,7 @@ void loadCrazyflieContext()
IntWithHeader msg;
msg.shouldCheckForAgentID = false;
msg.data = CMD_DISCONNECT;
- crazyRadioCommandPublisher.publish(msg);
+ m_crazyRadioCommandPublisher.publish(msg);
}
m_context = new_context;
@@ -1079,7 +1079,7 @@ void loadCrazyflieContext()
void createControllerServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient )
{
ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
- ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "FlyingAgentClientConfig");
std::string controllerName;
if(!nodeHandle.getParam(paramter_name, controllerName))
@@ -1096,7 +1096,7 @@ void createControllerServiceClientFromParameterName( std::string paramter_name ,
void createIntIntServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient )
{
ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
- ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "FlyingAgentClientConfig");
std::string controllerName;
if(!nodeHandle.getParam(paramter_name, controllerName))
@@ -1157,7 +1157,7 @@ void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
// ----------------------------------------------------------------------------------
-void isReadyClientConfigYamlCallback(const IntWithHeader & msg)
+void isReadyFlyingAgentClientConfigYamlCallback(const IntWithHeader & msg)
{
// Check whether the message is relevant
bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
@@ -1175,14 +1175,14 @@ void isReadyClientConfigYamlCallback(const IntWithHeader & msg)
// > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
case LOAD_YAML_FROM_AGENT:
{
- ROS_INFO("[FLYING AGENT CLIENT] Now fetching the ClientConfig YAML parameter values from this agent.");
+ ROS_INFO("[FLYING AGENT CLIENT] Now fetching the FlyingAgentClientConfig 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("[FLYING AGENT CLIENT] Now fetching the ClientConfig YAML parameter values from this agent's coordinator.");
+ ROS_INFO("[FLYING AGENT CLIENT] Now fetching the FlyingAgentClientConfig YAML parameter values from this agent's coordinator.");
namespace_to_use = m_namespace_to_coordinator_parameter_service;
break;
}
@@ -1197,20 +1197,20 @@ void isReadyClientConfigYamlCallback(const IntWithHeader & msg)
// Create a node handle to the selected parameter service
ros::NodeHandle nodeHandle_to_use(namespace_to_use);
// Call the function that fetches the parameters
- fetchClientConfigYamlParameters(nodeHandle_to_use);
+ fetchFlyingAgentClientConfigYamlParameters(nodeHandle_to_use);
}
}
// > Load the paramters from the Client Config YAML file
-void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
+void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
{
// Here we load the parameters that are specified in the file:
- // ClientConfig.yaml
+ // FlyingAgentClientConfig.yaml
- // Add the "ClientConfig" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "ClientConfig");
+ // Add the "FlyingAgentClientConfig" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "FlyingAgentClientConfig");
// Flag for whether to use angle for switching to the Safe Controller
yaml_isEnabled_strictSafety = getParameterBool(nodeHandle_for_paramaters,"isEnabled_strictSafety");
@@ -1232,16 +1232,16 @@ void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
yaml_shouldPerfom_landing_with_defaultController = getParameterBool(nodeHandle_for_paramaters,"shouldPerfom_landing_with_defaultController");
// DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] DEBUGGING: the fetched ClientConfig/isEnabled_strictSafety = " << yaml_isEnabled_strictSafety);
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] DEBUGGING: the fetched FlyingAgentClientConfig/isEnabled_strictSafety = " << yaml_isEnabled_strictSafety);
// PROCESS THE PARAMTERS
// Convert from degrees to radians
- m_maxTiltAngle_for_strictSafety_radians = DEG2RAD * yaml_maxTiltAngle_for_strictSafety_degrees;
+ yaml_maxTiltAngle_for_strictSafety_radians = DEG2RAD * yaml_maxTiltAngle_for_strictSafety_degrees;
// DEBUGGING: Print out one of the processed values
- ROS_INFO_STREAM("[FLYING AGENT CLIENT CONTROLLER] DEBUGGING: after processing m_maxTiltAngle_for_strictSafety_radians = " << m_maxTiltAngle_for_strictSafety_radians);
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT CONTROLLER] DEBUGGING: after processing yaml_maxTiltAngle_for_strictSafety_radians = " << yaml_maxTiltAngle_for_strictSafety_radians);
}
@@ -1259,70 +1259,91 @@ void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
int main(int argc, char* argv[])
{
- // Starting the ROS-node
+ // Starting the ROS-node
ros::init(argc, argv, "FlyingAgentClient");
- // Create a "ros::NodeHandle" type local variable named "nodeHandle",
- // the "~" indcates that "self" is the node handle assigned.
+ // 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 node
- std::string m_namespace = ros::this_node::getNamespace();
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] ros::this_node::getNamespace() = " << m_namespace);
+ // Get the namespace of this node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] ros::this_node::getNamespace() = " << m_namespace);
- // AGENT ID AND COORDINATOR ID
+ // AGENT ID AND COORDINATOR ID
- // Get the ID of the agent and its coordinator
- bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+ // 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.
- // Stall the node IDs are not valid
- if ( !isValid_IDs )
- {
- ROS_ERROR("[FLYING AGENT CLIENT] Node NOT FUNCTIONING :-)");
- ros::spin();
- }
- else
- {
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
- }
+ // 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("[FLYING AGENT CLIENT] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
- // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
- // Set the class variable "m_namespace_to_own_agent_parameter_service",
- // i.e., the namespace of parameter service for this agent
- m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
- // Set the class variable "m_namespace_to_coordinator_parameter_service",
- // i.e., the namespace of parameter service for this agent's coordinator
- constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
- // Inform the user of what namespaces are being used
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+ // 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.
- // Create, as local variables, node handles to the parameters services
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
- ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+ // Set the class variable "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("[FLYING AGENT CLIENT] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
- // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
- // The parameter service publishes messages with names of the form:
- // /dfall/.../ParameterService/<filename with .yaml extension>
- ros::Subscriber clientConfig_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "ClientConfig", 1, isReadyClientConfigYamlCallback);
- ros::Subscriber clientConfig_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("ClientConfig", 1, isReadyClientConfigYamlCallback);
- //ros::Subscriber safeController_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "SafeController", 1, isReadySafeControllerYamlCallback);
- //ros::Subscriber safeController_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("SafeController", 1, isReadySafeControllerYamlCallback);
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber flyingAgentClientConfig_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "FlyingAgentClientConfig", 1, isReadyFlyingAgentClientConfigYamlCallback);
+ ros::Subscriber flyingAgentClientConfig_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("FlyingAgentClientConfig", 1, isReadyFlyingAgentClientConfigYamlCallback);
- // GIVE YAML VARIABLES AN INITIAL VALUE
+ // GIVE YAML VARIABLES AN INITIAL VALUE
// This can be done either here or as part of declaring the
// variables in the header file
@@ -1331,183 +1352,174 @@ int main(int argc, char* argv[])
// FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
- // Call the class function that loads the parameters for this class.
- fetchClientConfigYamlParameters(nodeHandle_to_own_agent_parameter_service);
- //fetchSafeControllerYamlParameters(nodeHandle_to_own_agent_parameter_service);
-
-
-
-
-
- // INITIALISE ALL THE CONTROLLER SERVICE CLIENTS
- // > This cannot be done directly here because the other nodes may
- // be currently unavailable. Hence, we start a timer for a few
- // seconds and in the call back all the controller service
- // clients are created.
- m_controllers_avialble = false;
- m_timer_for_createAllControllerServiceClients = nodeHandle.createTimer(ros::Duration(3), timerCallback_for_createAllcontrollerServiceClients, true);
-
+ // Call the class function that loads the parameters
+ // from the "FlyingAgentClientConfig.yaml" file.
+ // > This is possible because that YAML file is added
+ // to the parameter service when launched via the
+ // "agent.launch" file.
+ fetchFlyingAgentClientConfigYamlParameters(nodeHandle_to_own_agent_parameter_service);
- // INITIALISE THE MOTION CAPTURE TIME-OUT TIMER
- // > And stop it immediately
- m_isAvailable_mocap_data = false;
- m_timer_mocap_timeout_check = nodeHandle.createTimer(ros::Duration(yaml_mocap_timeout_duration), timerCallback_mocap_timeout_check, true);
- m_timer_mocap_timeout_check.stop();
-
- // INITIALISE THE TAKE-OFF AND LANDING COMPLETE TIMERS
- // > And stop it immediately
- m_timer_takeoff_complete = nodeHandle.createTimer(ros::Duration(1.0), timerCallback_takeoff_complete, true);
- m_timer_takeoff_complete.stop();
- m_timer_land_complete = nodeHandle.createTimer(ros::Duration(1.0), timerCallback_takeoff_complete, true);
- m_timer_land_complete.stop();
+ // INITIALISE ALL THE CONTROLLER SERVICE CLIENTS
+ // > This cannot be done directly here because the other nodes may
+ // be currently unavailable. Hence, we start a timer for a few
+ // seconds and in the call back all the controller service
+ // clients are created.
+ m_controllers_avialble = false;
+ m_timer_for_createAllControllerServiceClients = nodeHandle.createTimer(ros::Duration(3), timerCallback_for_createAllcontrollerServiceClients, true);
+ // INITIALISE THE MOTION CAPTURE TIME-OUT TIMER
+ // > And stop it immediately
+ m_isAvailable_mocap_data = false;
+ m_timer_mocap_timeout_check = nodeHandle.createTimer(ros::Duration(yaml_mocap_timeout_duration), timerCallback_mocap_timeout_check, true);
+ m_timer_mocap_timeout_check.stop();
- // PUBLISHERS, SUBSCRIBERS, AND SERVICE CLIENTS
-
+ // INITIALISE THE TAKE-OFF AND LANDING COMPLETE TIMERS
+ // > And stop it immediately
+ m_timer_takeoff_complete = nodeHandle.createTimer(ros::Duration(1.0), timerCallback_takeoff_complete, true);
+ m_timer_takeoff_complete.stop();
+ m_timer_land_complete = nodeHandle.createTimer(ros::Duration(1.0), timerCallback_takeoff_complete, true);
+ m_timer_land_complete.stop();
- // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
- ros::NodeHandle nodeHandle_dfall_root("/dfall");
- // 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);
+ // INITIALISE THE CRAZY RADIO STATUS
+ m_crazyradio_status = CRAZY_RADIO_STATE_DISCONNECTED;
- // SERVICE CLIENT FOR LOADING THE ALLOCATED FLYING ZONE
- // AND OTHER CONTEXT DETAILS
+ // INITIALISE THE FLYING STATE AND PUBLISH
+ m_flying_state = STATE_MOTORS_OFF;
- //ros::service::waitForService("/CentralManagerService/CentralManager");
- centralManager = nodeHandle_dfall_root.serviceClient<CMQuery>("CentralManagerService/Query", false);
- loadCrazyflieContext();
- // Subscriber for when the Flying Zone Database changed
- ros::Subscriber databaseChangedSubscriber = nodeHandle_dfall_root.subscribe("CentralManagerService/DBChanged", 1, crazyflieContextDatabaseChangedCallback);
- // EMERGENCY STOP OF THE WHOLE "D-FaLL-System"
- ros::Subscriber emergencyStopSubscriber = nodeHandle_dfall_root.subscribe("emergencyStop", 1, emergencyStopReceivedCallback);
- // LOCALISATION DATA FROM MOTION CAPTURE SYSTEM
- //keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
- ros::Subscriber viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, viconCallback);
+ // PUBLISHERS, SUBSCRIBERS, AND SERVICE CLIENTS
- // PUBLISHER FOR COMMANDING THE CRAZYFLIE
- // i.e., motorCmd{1,2,3,4}, roll, pitch, yaw, and onboardControllerType
- //ros::Publishers to advertise the control output
- m_commandForSendingToCrazyfliePublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
- //this topic lets the FlyingAgentClient listen to the terminal commands
- //commandPublisher = nodeHandle.advertise<std_msgs::Int32>("Command", 1);
+ // CREATE A NODE HANDLE TO THE COORDINATOR
+ std::string namespace_to_coordinator;
+ constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
- // SUBSCRIBER FOR THE CHANGE STATE COMMANDS
- // i.e., {TAKE-OFF,LAND,MOTORS-OFF,CONTROLLER SELECTION}
- // > for the agent GUI
- ros::Subscriber commandSubscriber_to_agent = nodeHandle.subscribe("Command", 1, flyingStateRequestCallback);
- // > for the coord GUI
- ros::Subscriber commandSubscriber_to_coord = nodeHandle_to_coordinator.subscribe("FlyingAgentClient/Command", 1, flyingStateRequestCallback);
+ // 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);
+ // EMERGENCY STOP OF THE WHOLE "D-FaLL-System"
+ ros::Subscriber emergencyStopSubscriber = nodeHandle_dfall_root.subscribe("emergencyStop", 1, emergencyStopReceivedCallback);
- // PUBLISHER FOR THE CRAZYRADIO COMMANDS
- // i.e., {CONNECT,DISCONNECT}
- // This topic lets us use the terminal to communicate with
- // the crazyRadio node even when the GUI is not launched
- crazyRadioCommandPublisher = nodeHandle.advertise<IntWithHeader>("crazyRadioCommand", 1);
+ // LOCALISATION DATA FROM MOTION CAPTURE SYSTEM
+ //keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
+ ros::Subscriber viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, viconCallback);
- // PUBLISHER FOR THE FLYING STATE
- // Possible states: {MOTORS-OFF,TAKE-OFF,FLYING,LAND}
- // This topic will publish flying state whenever it changes.
- m_flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
+ // PUBLISHER FOR COMMANDING THE CRAZYFLIE
+ // i.e., motorCmd{1,2,3,4}, roll, pitch, yaw, and onboardControllerType
+ //ros::Publishers to advertise the control output
+ m_commandForSendingToCrazyfliePublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
-
- // PUBLISHER FOR THE
- controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerUsed", 1);
+ // SUBSCRIBER FOR THE CHANGE STATE COMMANDS
+ // i.e., {TAKE-OFF,LAND,MOTORS-OFF,CONTROLLER SELECTION}
+ // > for the agent GUI
+ ros::Subscriber commandSubscriber_to_agent = nodeHandle.subscribe("Command", 1, flyingStateRequestCallback);
+ // > for the coord GUI
+ ros::Subscriber commandSubscriber_to_coord = nodeHandle_to_coordinator.subscribe("FlyingAgentClient/Command", 1, flyingStateRequestCallback);
+ // PUBLISHER FOR THE CRAZYRADIO COMMANDS
+ // i.e., {CONNECT,DISCONNECT}
+ // This topic lets us use the terminal to communicate with
+ // the crazyRadio node even when the GUI is not launched
+ m_crazyRadioCommandPublisher = nodeHandle.advertise<IntWithHeader>("crazyRadioCommand", 1);
- // crazy radio status
- m_crazyradio_status = CRAZY_RADIO_STATE_DISCONNECTED;
+ // PUBLISHER FOR THE FLYING STATE
+ // Possible states: {MOTORS-OFF,TAKE-OFF,FLYING,LAND}
+ // This topic will publish flying state whenever it changes.
+ m_flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
- // publish first flying state data
- std_msgs::Int32 flying_state_msg;
- flying_state_msg.data = m_flying_state;
- m_flyingStatePublisher.publish(flying_state_msg);
- // SafeControllerServicePublisher:
- ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
- //safeControllerServiceSetpointPublisher = namespaceNodeHandle.advertise<dfall_pkg::Setpoint>("SafeControllerService/Setpoint", 1);
- //ros::Subscriber controllerSetpointSubscriber = namespaceNodeHandle.subscribe("student_GUI/ControllerSetpoint", 1, controllerSetPointCallback);
- //ros::Subscriber safeSetpointSubscriber = namespaceNodeHandle.subscribe("SafeControllerService/Setpoint", 1, safeSetPointCallback);
+ // PUBLISHER FOR THE CONTROLLER CURRENTLY IN USE
+ // This publishes the "m_instant_controller" variable
+ // to reflect the controller that is actually called
+ // when motion capture data is received.
+ m_controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerUsed", 1);
-
-
- // crazyradio status. Connected, connecting or disconnected
- ros::Subscriber crazyRadioStatusSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, crazyRadioStatusCallback);
+ // SUBSCRIBER FOR CRAZY RADIO STATUS CHANGES
+ // crazyradio status. Connected, connecting or disconnected
+ std::string namespace_to_crazy_radio = m_namespace + "/CrazyRadio";
+ ros::NodeHandle nodeHandle_to_crazy_radio(namespace_to_crazy_radio);
+ ros::Subscriber crazyRadioStatusSubscriber = nodeHandle_to_crazy_radio.subscribe("CrazyRadio/CrazyRadioStatus", 1, crazyRadioStatusCallback);
+ // SUBSCRIBER FOR BATTERY STATE CHANGES
+ // The battery state change message from the Battery
+ // Monitor node
+ std::string namespace_to_battery_monitor = m_namespace + "/BatteryMonitor";
+ ros::NodeHandle nodeHandle_to_battery_monitor(namespace_to_battery_monitor);
+ ros::Subscriber CFBatterySubscriber = nodeHandle_to_battery_monitor.subscribe("ChangedStateTo", 1, batteryMonitorStateChangedCallback);
+ // SERVICE SERVER FOR OTHERS TO GET THE CURRENT FLYING STATE
+ // Advertise the service that return the "m_flying_state"
+ // variable when called upon
+ ros::ServiceServer getCurrentFlyingStateService = nodeHandle.advertiseService("getCurrentFlyingState", getCurrentFlyingStateServiceCallback);
- // will publish battery state when it changes
- //batteryStatePublisher = nodeHandle.advertise<std_msgs::Int32>("batteryState", 1);
- // INITIALISE THE BATTERY VOLTAGE TO SOMETHING CLOSE TO FULL
- // > This is used to prevent the "Low Battery" being trigged when the
- // first battery voltage data is received
- //m_battery_voltage = 4.2f;
- // know the battery level of the CF
- //ros::Subscriber CFBatterySubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CFBattery", 1, CFBatteryCallback);
- //setBatteryStateTo(BATTERY_STATE_NORMAL); //initialize battery state
- // Subscribe to the battery state change message from the Battery Monitor node
- std::string namespace_to_battery_monitor = m_namespace + "/BatteryMonitor";
- ros::NodeHandle nodeHandle_to_battery_monitor(namespace_to_battery_monitor);
- ros::Subscriber CFBatterySubscriber = nodeHandle_to_battery_monitor.subscribe("ChangedStateTo", 1, batteryMonitorStateChangedCallback);
+ // PUBLISH THE FLYING STATE
+ // Ideally the GUI receives this message
+ std_msgs::Int32 flying_state_msg;
+ flying_state_msg.data = m_flying_state;
+ m_flyingStatePublisher.publish(flying_state_msg);
- //start with safe controller
- m_flying_state = STATE_MOTORS_OFF;
- setControllerNominallySelected(DEFAULT_CONTROLLER);
- setInstantController(DEFAULT_CONTROLLER); //initialize this also, so we notify GUI
+ // SET THE INITIAL CONTROLLER
+ // This cannot be done before the publishers because
+ // the function also sets the "m_instant_controller"
+ // (as the "m_flying_state" is "STATE_MOTORS_OFF")
+ // and the function that sets the instant controller
+ // publishes a message with the information.
+ setControllerNominallySelected(DEFAULT_CONTROLLER);
- // Advertise the service for the current flying state
- ros::ServiceServer getCurrentFlyingStateService = nodeHandle.advertiseService("getCurrentFlyingState", getCurrentFlyingStateServiceCallback);
- // Open a ROS "bag" to store data for post-analysis
- // std::string package_path;
- // package_path = ros::package::getPath("dfall_pkg") + "/";
- // ROS_INFO_STREAM(package_path);
- // std::string record_file = package_path + "LoggingFlyingAgentClient.bag";
- // bag.open(record_file, rosbag::bagmode::Write);
- ros::spin();
+ // Print out some information to the user.
+ ROS_INFO("[FLYING AGENT CLIENT] Ready :-)");
- // Close the ROS "bag" that was opened to store data for post-analysis
- //bag.close();
+ // Enter an endless while loop to keep the node alive.
+ ros::spin();
+ // Return zero if the "ross::spin" is cancelled.
return 0;
}
--
GitLab