DroneXControllerService.h

//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
//
//    This file is part of D-FaLL-System.
//
//    D-FaLL-System is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    D-FaLL-System is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
//
//
//    ----------------------------------------------------------------------------------
//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
//
//
//    DESCRIPTION:
//    Place for students to implement their controller
//
//    ----------------------------------------------------------------------------------





//    ----------------------------------------------------------------------------------
//    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>

//the generated structs from the msg-files have to be included
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomButton.h"
#include "d_fall_pps/CrazyflieContext.h"

// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"

#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>





//    ----------------------------------------------------------------------------------
//    DDDD   EEEEE  FFFFF  III  N   N  EEEEE   SSSS
//    D   D  E      F       I   NN  N  E      S
//    D   D  EEE    FFF     I   N N N  EEE     SSS
//    D   D  E      F       I   N  NN  E          S
//    DDDD   EEEEE  F      III  N   N  EEEEE  SSSS
//    ----------------------------------------------------------------------------------

// These constants are defined to make the code more readable and adaptable.

// Universal constants
#define PI 3.1415926535

#define RAD2DEG 180.0/PI
#define DEG2RAD PI/180.0



// FOR WHICH BUTTON WAS PRESSED IN THE DRONEX CONTOLLER
#define DRONEX_BUTTON_GOTOSTART     1
#define DRONEX_BUTTON_CONNECT       2
#define DRONEX_BUTTON_PICKUP        3
#define DRONEX_BUTTON_GOTOEND       4
#define DRONEX_BUTTON_PUTDOWN       5
#define DRONEX_BUTTON_SQUAT         6
#define DRONEX_BUTTON_DISCONNECT    7
#define DRONEX_TAKE_OFF             8
#define DRONEX_LAND                 9
#define DRONEX_ABORT                10

#define DRONEX_BUTTON_1             11
#define DRONEX_BUTTON_2             12
#define DRONEX_BUTTON_3             13
#define DRONEX_BUTTON_4             14

#define DRONEX_INTEGRATOR_ON    	  15
#define DRONEX_INTEGRATOR_OFF     	16
#define DRONEX_INTEGRATOR_RESET 	  17
#define DRONEX_FOLLOW_TRAJECTORY    18
#define DRONEX_RESET                19

#define DRONEX_STATE_GROUND			          0
#define DRONEX_STATE_TAKING_OFF		        1
#define DRONEX_STATE_LAND_ON_GROUND		    2
#define DRONEX_STATE_APPROACH		          3
#define DRONEX_STATE_HOVER                4
#define DRONEX_STATE_LAND_ON_MOTHERSHIP   5
#define DRONEX_STATE_ON_MOTHERSHIP        6
#define DRONEX_STATE_FOLLOWING_TRAJECTORY  7





// These constants define the modes that can be used for controller the Crazyflie 2.0,
// the constants defined here need to be in agreement with those defined in the
// firmware running on the Crazyflie 2.0.
// The following is a short description about each mode:
// MOTOR_MODE    In this mode the Crazyflie will apply the requested 16-bit per motor
//               command directly to each of the motors
// RATE_MODE     In this mode the Crazyflie will apply the requested 16-bit per motor
//               command directly to each of the motors, and additionally request the
//               body frame roll, pitch, and yaw angular rates from the PID rate
//               controllers implemented in the Crazyflie 2.0 firmware.
// ANGE_MODE     In this mode the Crazyflie will apply the requested 16-bit per motor
//               command directly to each of the motors, and additionally request the
//               body frame roll, pitch, and yaw angles from the PID attitude
//               controllers implemented in the Crazyflie 2.0 firmware.
#define CF_COMMAND_TYPE_MOTOR   6
#define CF_COMMAND_TYPE_RATE    7
#define CF_COMMAND_TYPE_ANGLE   8

// 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
//       Takes the poisition and angles directly as measured,
//       and estimates the velocities as a finite different to the
//       previous measurement
//
// ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION
//       Uses a 2nd order random walk estimator independently for
//       each of (x,y,z,roll,pitch,yaw)
//
// ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED
//       Uses the model of the quad-rotor and the previous inputs
//
#define ESTIMATOR_METHOD_FINITE_DIFFERENCE          1
#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION   2   // (DEFAULT)
#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED      3

// Flying states
#define STATE_MOTORS_OFF 1
#define STATE_TAKE_OFF   2
#define STATE_FLYING     3
#define STATE_LAND       4

// Namespacing the package
using namespace d_fall_pps;

// Flight sequences
#define SEQUENCE_NONE 					0
#define SEQUENCE_LAND_ON_MOTHERSHIP 	1
#define SEQUENCE_HOVER_OVER_MOTHERSHIP 	2





//    ----------------------------------------------------------------------------------
//    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
//    ----------------------------------------------------------------------------------

// Current time
int m_time_ticks = 0;
float m_time_seconds;

// > Mass of the Crazyflie quad-rotor, in [grams]
float m_mass_CF_grams;

Setpoint dronexSetpoint;
int flying_state = DRONEX_STATE_GROUND;

int integratorFlag = DRONEX_INTEGRATOR_OFF;
float integrator_sum_XYZ[3] = {0,0,0};


// Controller Mode
// 0:
// 1: Nested Controller
// 2: trajectory Follower
int controller_mode = 2;

// Variable for choosing flight sequence
int flightSequence = 0;

// Flags of landing sequence
bool tookOffFlag = false;
bool approachedFlag = false;
bool landedFlag = false;

// Flag for saving starting coordinates
bool savedStartCoordinates = false;
float startCoordinateX;
float startCoordinateY;
float startCoordinateZ;

// tolerances (x,y,z)
float tol_takeoff[3] =	{0.07, 0.07, 0.07};	//{0.03, 0.03, 0.03};
float tol_approach[3] =	{0.3, 0.3, 0.07};	// added to differ between approach and land *
float tol_land[3] = 	{0.03, 0.03, 0.03};	// *




// Origin of the Flying zone
float trajectory_setpoint[4]; // x,y,z,yaw
float trajectory_velocity[3]; // x,y,z
float originX = 0;
float originY = 0;
d_fall_pps::AreaBounds area;
double trajectory_start_time = 0;
double total_time_since_start = 0;
float trajectory_x_radius = 0;
float trajectory_y_radius = 0;
float trajectory_period = 5; // 1 round in 5 sec


















//describes the area of the crazyflie and other parameters
//CrazyflieContext context;

// > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
float m_mass_total_grams;
float gravity = 9.81;

// > The setpoints for (x,y,z) position and yaw angle, in that order
float m_setpoint[4] = {0.0,0.0,0.0,0.0};
float m_setpoint_for_controller[4] = {0.0,0.0,0.0,0.0};

// > Small adjustments to the x-y setpoint
float m_xAdjustment = 0.0f;
float m_yAdjustment = 0.0f;

// Boolean for whether to limit rate of change of the setpoint
bool m_shouldSmoothSetpointChanges = true;

// Max setpoint change per second
float m_max_setpoint_change_per_second_horizontal;
float m_max_setpoint_change_per_second_vertical;
float m_max_setpoint_change_per_second_yaw_degrees;
float m_max_setpoint_change_per_second_yaw_radians;


// Frequency at which the controller is running
float m_vicon_frequency;


float prev_DroneX_pos[3] = {0,0,0};
float current_DroneX_pos[3];
float drone_X_vel[3];


// THE FOLLOWING PARAMETERS ARE USED
// FOR THE LOW-LEVEL CONTROLLER

// Frequency at which the controller is running
float control_frequency;

// > Coefficients of the 16-bit command to thrust conversion
std::vector<float> motorPoly(3);

// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
std::vector<float> gainMatrixThrust_NineStateVector (9,0.0);
std::vector<float> gainMatrixRollRate               (9,0.0);
std::vector<float> gainMatrixPitchRate              (9,0.0);
std::vector<float> gainMatrixYawRate                (9,0.0);

std::vector<float> gainMatrixYawRatefromAngle 		(9,0.0);
std::vector<float> gainMatrixPitchRatefromAngle     (9,0.0);
std::vector<float> gainMatrixRollRatefromAngle      (9,0.0);

std::vector<float> gainMatrixThrust_SixStateVector 	(9,0.0);
std::vector<float> gainMatrixRollAngle               (9,0.0);
std::vector<float> gainMatrixPitchAngle              (9,0.0);


// Integrator parameters
std::vector<float> gainIntegrator					(3,0.0);	// in case we want to go back to old integrator concept, else TODO: delete
std::vector<float> gainIntegratorRate					(3,0.0);
std::vector<float> gainIntegratorAngle					(3,0.0);


// The 16-bit command limits
float cmd_sixteenbit_min;
float cmd_sixteenbit_max;


// VARIABLES FOR THE ESTIMATOR

// Frequency at which the controller is running
float estimator_frequency;

// > A flag for which estimator to use:
int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
// > The current state interial estimate,
//   for use by the controller
float current_stateInertialEstimate[12];

// > The measurement of the Crazyflie at the "current" time step,
//   to avoid confusion
float current_xzy_rpy_measurement[6];

// > The measurement of the Crazyflie at the "previous" time step,
//   used for computing finite difference velocities
float previous_xzy_rpy_measurement[6];

// > The full 12 state estimate maintained by the finite
//   difference state estimator
float stateInterialEstimate_viaFiniteDifference[12];


// > The full 12 state estimate maintained by the point mass
//   kalman filter state estimator
float stateInterialEstimate_viaPointMassKalmanFilter[12];

// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0);
std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0);
std::vector<float> PMKF_Kinf_for_positions      (2,0.0);
// > For the (roll,pitch,yaw) angles
std::vector<float> PMKF_Ahat_row1_for_angles    (2,0.0);
std::vector<float> PMKF_Ahat_row2_for_angles    (2,0.0);
std::vector<float> PMKF_Kinf_for_angles         (2,0.0);



// VARIABLES FOR THE NAMESPACES FOR THE PARAMETER SERVICES
// > For the paramter service of this agent
std::string namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
std::string namespace_to_coordinator_parameter_service;


// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
ros::Publisher debugPublisher;


// VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME

// Boolean whether to execute the convert into body frame function
bool shouldPerformConvertIntoBodyFrame = false;


// VARIABLES RELATING TO THE PUBLISHING OF A DEBUG MESSAGE

// Boolean indiciating whether the "Debug Message" of this agent should be published or not
bool shouldPublishDebugMessage = false;

// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
bool shouldDisplayDebugInfo = false;


// VARIABLES RELATING TO PUBLISHING CURRENT POSITION AND FOLLOWING ANOTHER AGENT'S
// POSITION

// The ID of this agent, i.e., the ID of this compute
int my_agentID = 0;

// Boolean indicating whether the (x,y,z,yaw) of this agent should be published or not
// > The default behaviour is: do not publish,
// > This varaible is changed based on parameters loaded from the YAML file
bool shouldPublishCurrent_xyz_yaw = false;


// ROS Publisher for my current (x,y,z,yaw) position
ros::Publisher my_current_xyz_yaw_publisher;

// ROS Publisher for the current setpoint
ros::Publisher dronexSetpointToGUIPublisher;
// publisher for flying state
ros::Publisher flyingStatePublisher;

// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
// The "CrazyflieData" type used for the "request" variable is a
// structure as defined in the file "CrazyflieData.msg" which has the following
// properties:
//     string crazyflieName              The name given to the Crazyflie in the Vicon software
//     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





//    ----------------------------------------------------------------------------------
//    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.


// ADDED FOR THE DRONEX
void perControlCycleOperations();

// CALLBACK FROM ROS MESSAGES RECEIVED
void buttonPressedCallback(const std_msgs::Int32& msg);
void dronexCoordinateCallback(const ViconData& viconData);
//void coordinatesToLocal(CrazyflieData& cf);


// SEPARATE CALLBACK FUNCTIONS FOR EACH BUTTON
void buttonPressed_gotoStart();
void buttonPressed_connect();
void buttonPressed_pickup();
void buttonPressed_gotoEnd();
void buttonPressed_putdown();
void buttonPressed_squat();
void buttonPressed_disconnect();

void buttonPressed_1();
void buttonPressed_2();
void buttonPressed_3();
void buttonPressed_4();

void buttonPressed_take_off();
void buttonPressed_land();
void buttonPressed_abort();

void buttonPressed_integrator_on();
void buttonPressed_integrator_off();
void buttonPressed_integrator_reset();
void buttonPressed_follow_trajectory();

void buttonPressed_reset();


void calculateTrajectory();
void crazyfliecontextRefresh(d_fall_pps::CrazyflieContext context);
void motorsOFF(Controller::Response &response);

// CONTROLLER COMPUTATIONS
// > The function that is called to "start" all estimation and control computations
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response);
// > The various functions that implement an LQR controller
void calculateControlOutput_viaLQR(                     float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforMotors(            float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforActuators(         float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforRates(             float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforAngles(            float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaAngleResponseTest(       float stateErrorBody[12], Controller::Request &request, Controller::Response &response);


void calculateControlOutput_viaLQRforRates_forLanding(	float stateErrorBody[12], Controller::Request &request, Controller::Response &response);

// Velocity estimator
void calculateDroneXVelocity(Controller::Request &request);

// Integrator
void integrator_XYZ(float (&stateErrorBody)[12]);

// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();


// PUBLISHING OF THE DEBUG MESSAGE
void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);


// TRANSFORMATION FROM INTERIAL ESTIMATE TO BODY FRAME ERROR
void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12]);

// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR
void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured);

// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
float computeMotorPolyBackward(float thrust);

// SETPOINT CHANGE CALLBACK
void setpointCallback(const Setpoint& newSetpoint);
void xyz_yaw_to_follow_callback(const Setpoint& newSetpoint);

void publishCurrentSetpoint();

// CUSTOM COMMAND RECEIVED CALLBACK
//void customCommandReceivedCallback(const CustomButton& commandReceived);

// PUBLISH THE CURRENT X,Y,Z, AND YAW
void publish_current_xyz_yaw(float x, float y, float z, float yaw);

// LOAD PARAMETERS
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
void  getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
int   getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
void  getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
int   getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
bool  getParameterBool(ros::NodeHandle& nodeHandle, std::string name);

void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
void fetchYamlParameters(ros::NodeHandle& nodeHandle);
void processFetchedParameters();