Commit c6a3ca1d authored by pragash1's avatar pragash1
Browse files

Button Pressed Event implemented

parent 95389c8e
......@@ -139,6 +139,12 @@
#define PICKER_BUTTON_4 14
// FOR WHICH BUTTON WAS PRESSED IN THE DRONEX CONTOLLER
#define DRONEX_TAKE_OFF 8
#define DRONEX_LAND 9
// Universal constants
#define PI 3.141592653589
......@@ -334,8 +340,10 @@ private:
ros::Publisher tuningHeadingGainPublisher;
// > For the DRONEX CONTROLLER
ros::Publisher droneXSetpointPublisher;
ros::Subscriber droneXSetpointSubscriber;
ros::Publisher dronexButtonPressedPublisher;
ros::Publisher dronexSetpointPublisher;
ros::Subscriber dronexSetpointSubscriber;
ros::Subscriber dronexSetpointToGUISubscriber;
......@@ -348,7 +356,7 @@ private:
ros::Publisher pickerYAdjustmentPublisher;
ros::Publisher pickerSetpointPublisher;
ros::Subscriber pickerSetpointSubscriber;
ros::Subscriber pickerSetpointToGUISubscriber;
ros::Subscriber pickerSetpointToGUISubscriber;
......@@ -385,6 +393,7 @@ private:
void studentSetpointCallback(const Setpoint& newSetpoint);
void mpcSetpointCallback(const Setpoint& newSetpoint);
void pickerSetpointCallback(const Setpoint& newSetpoint);
void dronexSetpointCallback(const Setpoint& newSetpoint);
void remoteDataCallback(const CrazyflieData& objectData);
......@@ -393,6 +402,7 @@ private:
// > For actually sending the button message
void send_picker_button_clicked_message(int button_index);
void send_dronex_button_clicked_message(int button_index);
void DBChangedCallback(const std_msgs::Int32& msg);
......
......@@ -113,7 +113,10 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
//TODO add Publisher and Subscriber for DroneX
dronexButtonPressedPublisher = nodeHandle.advertise<std_msgs::Int32>("DroneXControllerService/ButtonPressed", 1);
dronexSetpointPublisher = nodeHandle.advertise<Setpoint>("DroneXControllerService/Setpoint", 1);
dronexSetpointSubscriber = nodeHandle.subscribe("DroneXControllerService/Setpoint", 1, &MainWindow::dronexSetpointCallback, this);
dronexSetpointToGUISubscriber = nodeHandle.subscribe("DroneXControllerService/SetpointToGUI", 1, &MainWindow::dronexSetpointCallback, this);
// SET ALL SLIDERS AND DIALS TO DEFAULT VALUES
......@@ -388,6 +391,7 @@ void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
case DRONEX_CONTROLLER:
highlightDroneXControllerTab();
default:
ROS_INFO("controllerUsedChangedCallback-Controller Flag not recognised");
break;
}
}
......@@ -441,6 +445,16 @@ void MainWindow::pickerSetpointCallback(const Setpoint& newSetpoint)
}
void MainWindow::dronexSetpointCallback(const Setpoint& newSetpoint)
{
m_picker_setpoint = newSetpoint;
ROS_INFO("dronexSetpointCallback");
// here we get the new setpoint, need to update it in GUI
//ui->picker_z_slider->setValue( int(newSetpoint.z*100.0) );
//ui->picker_yaw_dial->setValue( int(newSetpoint.yaw * RAD2DEG) );
}
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
{
// PUT THE CURRENT STATE INTO THE CLASS VARIABLE
......@@ -1525,23 +1539,7 @@ void MainWindow::on_enable_droneX_controller_clicked()
}
// TODO
// # Custom command buttons - FOR DRONEX CONTROLLER
void MainWindow::on_take_off_dronex_button_clicked(){
ROS_INFO("DroneX: Take off from DroneX! - Please upgrade my function in MainWindow.cpp!!!!!!!!!!!!!!!!!!!");
}
void MainWindow::on_land_on_dronex_button_clicked(){
ROS_INFO("DroneX: Land on DroneX - Please upgrade my function in MainWindow.cpp!!!!!!!!!!!!!!!!!!!");
ROS_INFO("Stefano and valentin were here");
std_msgs::Int32 msg;
msg.data = 10;
//this->droneXSetpointPublisher.publish(msg);
}
......@@ -1846,6 +1844,42 @@ void MainWindow::on_tuning_slider_heading_valueChanged(int value)
// TODO
//-----------------------------------------------------------------------------------------------------------------------------------------------
// # Custom command buttons - FOR DRONEX CONTROLLER
//-----------------------------------------------------------------------------------------------------------------------------------------------
// Publish Message that certain button was clicked
void MainWindow::send_dronex_button_clicked_message(int button_index){
// Initialise the message
std_msgs::Int32 msg;
// Set the msg data
msg.data = button_index;
// Publish the message
this-> dronexButtonPressedPublisher.publish(msg);
}
void MainWindow::on_take_off_dronex_button_clicked(){
//ROS_INFO("DroneX: Take off from DroneX! - Please upgrade my function in MainWindow.cpp!!!!!!!!!!!!!!!!!!!");
send_dronex_button_clicked_message(DRONEX_TAKE_OFF);
}
void MainWindow::on_land_on_dronex_button_clicked(){
//ROS_INFO("DroneX: Land on DroneX - Please upgrade my function in MainWindow.cpp!!!!!!!!!!!!!!!!!!!");
//ROS_INFO("Stefano and valentin were here");
send_dronex_button_clicked_message(DRONEX_LAND);
}
......
// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
// 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
......@@ -56,13 +56,13 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
//#include "d_fall_pps/CustomControllerYAML.h"
#include "d_fall_pps/CustomButton.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
......@@ -81,6 +81,28 @@
// 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_BUTTON_1 11
#define DRONEX_BUTTON_2 12
#define DRONEX_BUTTON_3 13
#define DRONEX_BUTTON_4 14
// 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.
......@@ -95,21 +117,32 @@
// command directly to each of the motors, and additionally request the
// body frame roll, pitch, and yaw angles from the PID attitude
// controllers implemented in the Crazyflie 2.0 firmware.
#define MOTOR_MODE 6
#define RATE_MODE 7
#define ANGLE_MODE 8
// These constants define the controller used for computing the response in the
// "calculateControlOutput" function
#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:
// LQR_RATE_MODE LQR controller based on the state vector:
// [position,velocity,angles]
//
// LQR_ANGLE_MODE LQR controller based on the state vector:
// [position,velocity]
// 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 LQR_RATE_MODE 1 // (DEFAULT)
#define LQR_ANGLE_MODE 2
#define ESTIMATOR_METHOD_FINITE_DIFFERENCE 1
#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION 2 // (DEFAULT)
#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
// Namespacing the package
using namespace d_fall_pps;
......@@ -126,37 +159,167 @@ using namespace d_fall_pps;
// V A A R R III A A BBBB LLLLL EEEEE SSSS
// ----------------------------------------------------------------------------------
// Variables for controller
float cf_mass; // Crazyflie mass in grams
std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
float control_frequency = 200.0; // Frequency at which the controller is running
float gravity_force = 0.0; // The weight of the Crazyflie in Newtons, i.e., mg
// Current time
int m_time_ticks = 0;
float m_time_seconds;
float previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
// > Mass of the Crazyflie quad-rotor, in [grams]
float m_mass_CF_grams;
std::vector<float> setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z) position and yaw angle, in that order
// > Mass of the letters to be lifted, in [grams]
float m_mass_E_grams;
float m_mass_T_grams;
float m_mass_H_grams;
// > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
float m_mass_total_grams;
// The LQR Controller parameters for "LQR_RATE_MODE"
std::vector<float> gainMatrixRollRate = { 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00};
std::vector<float> gainMatrixPitchRate = { 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00};
std::vector<float> gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
std::vector<float> gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.08, 0.00, 0.00, 0.00};
// Thickness of the object at pick-up and put-down, in [meters]
// > This should also account for extra height due to
// the surface where the object is
float m_thickness_of_object_at_pickup;
float m_thickness_of_object_at_putdown;
// (x,y) coordinates of the pickup location
std::vector<float> m_pickup_coordinates_xy(2);
// ROS Publisher for debugging variables
ros::Publisher debugPublisher;
// (x,y) coordinates of the drop off location
std::vector<float> m_dropoff_coordinates_xy_for_E(2);
std::vector<float> m_dropoff_coordinates_xy_for_T(2);
std::vector<float> m_dropoff_coordinates_xy_for_H(2);
// Length of the string from the Crazyflie
// to the end of the DroneX, in [meters]
float m_dronex_string_length;
// > The setpoints for (x,y,z) position and yaw angle, in that order
float m_setpoint[4] = {0.0,0.0,0.4,0.0};
float m_setpoint_for_controller[4] = {0.0,0.0,0.4,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;
// 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);
// 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];
// Variable for the namespaces for the paramter services
// 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;
// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
// The "CrazyflieData" type used for the "request" variable is a
......@@ -196,30 +359,86 @@ int my_agentID = 0;
// 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);
// 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();
// CONTROLLER COMPUTATIONS
// > The function that is called to "start" all estimation and control computations
bool calculateControlOutput(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);
// 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[9], float (&stateBody)[9], float yaw_measured);
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);
//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 getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
void fetchYamlParameters(ros::NodeHandle& nodeHandle);
void processFetchedParameters();
//void customYAMLasMessageCallback(const CustomControllerYAML& newCustomControllerParameters);
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment