To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 43fa29a6 authored by beuchatp's avatar beuchatp
Browse files

The test motors controller is now integrated to the flying agent client. The...

The test motors controller is now integrated to the flying agent client. The Default controller has the z integrator turned on, and the setpoint smoothing was slightly changed
parent 432d44ac
......@@ -328,6 +328,8 @@ add_executable(PickerControllerService src/nodes/PickerControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(TemplateControllerService src/nodes/TemplateControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(TestMotorsControllerService src/nodes/TestMotorsControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(ParameterService src/nodes/ParameterService.cpp)
......@@ -403,6 +405,7 @@ add_dependencies(RemoteControllerService dfall_pkg_generate_messages_cpp ${cat
add_dependencies(TuningControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(PickerControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(TemplateControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(TestMotorsControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CentralManagerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(ParameterService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
......@@ -455,6 +458,7 @@ target_link_libraries(RemoteControllerService ${catkin_LIBRARIES})
target_link_libraries(TuningControllerService ${catkin_LIBRARIES})
target_link_libraries(PickerControllerService ${catkin_LIBRARIES})
target_link_libraries(TemplateControllerService ${catkin_LIBRARIES})
target_link_libraries(TestMotorsControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(ParameterService ${catkin_LIBRARIES})
......
......@@ -116,6 +116,8 @@
#define PICKER_CONTROLLER 7
#define TEMPLATE_CONTROLLER 8
#define TESTMOTORS_CONTROLLER 21
// The constants that "command" changes in the
// operation state of this agent
#define CMD_USE_DEFAULT_CONTROLLER 1
......
......@@ -94,7 +94,7 @@ public:
void showHideController_tuning_changed();
void showHideController_template_changed();
void testMotors_changed();
void testMotors_triggered();
public slots:
......
......@@ -133,7 +133,7 @@ private slots:
void on_action_showHideController_tuning_changed();
void on_action_showHideController_template_changed();
void on_action_testMotors_changed();
void on_action_testMotors_triggered();
};
......
......@@ -118,7 +118,7 @@ void EnableControllerLoadYamlBar::showHideController_template_changed()
// TEST MOTORS
void EnableControllerLoadYamlBar::testMotors_changed()
void EnableControllerLoadYamlBar::testMotors_triggered()
{
#ifdef CATKIN_MAKE
dfall_pkg::IntWithHeader msg;
......
......@@ -253,14 +253,10 @@ void MainWindow::on_action_showHideController_template_changed()
}
void MainWindow::on_action_testMotors_changed()
void MainWindow::on_action_testMotors_triggered()
{
#ifdef CATKIN_MAKE
// Inform the user that the menu item was selected
ROS_INFO("[FLYING AGENT GUI MAIN WINDOW] Test Motors Menu Item clicked in Main Window.");
#endif
// Notify the UI elements of this change
ui->customWidget_enableControllerLoadYamlBar->testMotors_changed();
ui->customWidget_enableControllerLoadYamlBar->testMotors_triggered();
}
......
......@@ -129,6 +129,8 @@
#define PICKER_CONTROLLER 7
#define TEMPLATE_CONTROLLER 8
#define TESTMOTORS_CONTROLLER 21
// The constants that "command" changes in the
// operation state of this agent
#define CMD_USE_DEFAULT_CONTROLLER 1
......
......@@ -204,6 +204,8 @@ ros::ServiceClient m_pickerController;
// The Template controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_templateController;
// The Test Mtoros controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_testMotorsController;
......
// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// A controller to test the motors
//
// ----------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------
// III N N CCCC L U U DDDD EEEEE SSSS
// I NN N C L U U D D E S
// I N N N C L U U D D EEE SSS
// I N NN C L U U D D E S
// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
// ----------------------------------------------------------------------------------
// These various headers need to be included so that this controller service can be
// connected with the D-FaLL system.
//some useful libraries
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
#include <ros/package.h>
// Include the standard message types
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
#include <std_msgs/String.h>
// Include the DFALL message types
#include "dfall_pkg/IntWithHeader.h"
//#include "dfall_pkg/StringWithHeader.h"
#include "dfall_pkg/SetpointWithHeader.h"
#include "dfall_pkg/CustomButtonWithHeader.h"
#include "dfall_pkg/ViconData.h"
#include "dfall_pkg/Setpoint.h"
#include "dfall_pkg/ControlCommand.h"
#include "dfall_pkg/Controller.h"
#include "dfall_pkg/DebugMsg.h"
// Include the DFALL service types
#include "dfall_pkg/LoadYamlFromFilename.h"
#include "dfall_pkg/GetSetpointService.h"
// Include the shared definitions
#include "nodes/Constants.h"
// Include other classes
#include "classes/GetParamtersAndNamespaces.h"
// Need for having a ROS "bag" to store data for post-analysis
//#include <rosbag/bag.h>
// Namespacing the package
using namespace dfall_pkg;
// ----------------------------------------------------------------------------------
// DDDD EEEEE FFFFF III N N EEEEE SSSS
// D D E F I NN N E S
// D D EEE FFF I N N N EEE SSS
// D D E F I N NN E S
// DDDD EEEEE F III N N EEEEE SSSS
// ----------------------------------------------------------------------------------
// These constants are defined to make the code more readable and adaptable.
// NOTE: many constants are already defined in the
// "Constant.h" header file
// ----------------------------------------------------------------------------------
// V V A RRRR III A BBBB L EEEEE SSSS
// V V A A R R I A A B B L E S
// V V A A RRRR I A A BBBB L EEE SSS
// V V AAAAA R R I AAAAA B B L E S
// V A A R R III A A BBBB LLLLL EEEEE SSSS
// ----------------------------------------------------------------------------------
// The ID of the agent that this node is monitoring
int m_agentID;
// The ID of the agent that can coordinate this node
int m_coordID;
// NAMESPACES FOR THE PARAMETER SERVICES
// > For the paramter service of this agent
std::string m_namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
std::string m_namespace_to_coordinator_parameter_service;
// VARAIBLES FOR VALUES LOADED FROM THE YAML FILE
// > the mass of the crazyflie, in [grams]
float yaml_cf_mass_in_grams = 25.0;
// > the frequency at which the controller is running
float yaml_control_frequency = 200.0;
// > the coefficients of the 16-bit command to thrust conversion
//std::vector<float> yaml_motorPoly(3);
std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
// The min and max for saturating 16 bit thrust commands
float yaml_command_sixteenbit_min = 1000;
float yaml_command_sixteenbit_max = 60000;
// > the default setpoint, the ordering is (x,y,z,yaw),
// with units [meters,meters,meters,radians]
std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};
// Boolean indiciating whether the "Debug Message" of this agent should be published or not
bool yaml_shouldPublishDebugMessage = false;
// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
bool yaml_shouldDisplayDebugInfo = false;
// The LQR Controller parameters for "LQR_RATE_MODE"
std::vector<float> yaml_gainMatrixThrust_NineStateVector = { 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00};
std::vector<float> yaml_gainMatrixRollRate = { 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00};
std::vector<float> yaml_gainMatrixPitchRate = { 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00};
std::vector<float> yaml_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30};
// The weight of the Crazyflie in Newtons, i.e., mg
float m_cf_weight_in_newtons = 0.0;
// The location error of the Crazyflie at the "previous" time step
float m_previous_stateErrorInertial[9];
// The setpoint to be tracked, the ordering is (x,y,z,yaw),
// with units [meters,meters,meters,radians]
std::vector<float> m_setpoint{0.0,0.0,0.4,0.0};
// ROS Publisher for debugging variables
ros::Publisher m_debugPublisher;
// ROS Publisher for inform the network about
// changes to the setpoin
ros::Publisher m_setpointChangedPublisher;
// ----------------------------------------------------------------------------------
// FFFFF U U N N CCCC TTTTT III OOO N N
// F U U NN N C T I O O NN N
// FFF U U N N N C T I O O N N N
// F U U N NN C T I O O N NN
// F UUU N N CCCC T III OOO N N
//
// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
// P P R R O O T O O T Y Y P P E S
// PPPP RRRR O O T O O T Y PPPP EEE SSS
// P R R O O T O O T Y P E S
// P R R OOO T OOO T Y P EEEEE SSSS
// ----------------------------------------------------------------------------------
// These function prototypes are not strictly required for this code to
// complile, but adding the function prototypes here means the the functions
// can be written below in any order. If the function prototypes are not
// included then the function need to written below in an order that ensure
// each function is implemented before it is called from another function,
// hence why the "main" function is at the bottom.
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR
// INTO AN (x,y) BODY FRAME ERROR
void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);
// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
float computeMotorPolyBackward(float thrust);
// REQUEST SETPOINT CHANGE CALLBACK
void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
// CHANGE SETPOINT FUNCTION
void setNewSetpoint(float x, float y, float z, float yaw);
// GET CURRENT SETPOINT SERVICE CALLBACK
bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
// FOR LOADING THE YAML PARAMETERS
void isReadyTemplateControllerYamlCallback(const IntWithHeader & msg);
void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
......@@ -119,6 +119,15 @@
>
</node>
<!-- TEST MOTORS CONTROLLER -->
<node
pkg = "dfall_pkg"
name = "TestMotorsControllerService"
output = "screen"
type = "TestMotorsControllerService"
>
</node>
<!-- PARAMETER SERVICE -->
<node
pkg = "dfall_pkg"
......
......@@ -42,7 +42,7 @@ takoff_goto_setpoint_max_time: 4.0
takoff_integrator_on_box_horizontal: 0.25
takoff_integrator_on_box_vertical: 0.15
# The time for: take off integrator-on
takoff_integrator_on_time: 2.0
takoff_integrator_on_time: 3.0
# Height change for the landing move-down
......@@ -106,7 +106,7 @@ gainPitchRate_fromAngle : 4.00
# The LQR Controller parameters for yaw
gainYawRate_fromAngle : 2.30
# Integrator gains
integratorGain_forThrust : 0.00
integratorGain_forThrust : 0.10
integratorGain_forTauXY : 0.00
integratorGain_forTauYaw : 0.00
......
......@@ -6,6 +6,8 @@ tuningController: "TuningControllerService/TuningController"
pickerController: "PickerControllerService/PickerController"
templateController: "TemplateControllerService/TemplateController"
testMotorsController: "TestMotorsControllerService/TestMotorsController"
# The name of the service advertised by the Default
# Controller for requests manoeuvres to be performed
defaultController_requestManoeuvre: "DefaultControllerService/RequestManoeuvre"
......
......@@ -704,13 +704,6 @@ void smoothSetpointChanges( float target_setpoint[4] , float (&current_setpoint)
float max_for_z = yaml_max_setpoint_change_per_second_vertical / yaml_control_frequency;
// > Compute the current difference
float diff_for_z = target_setpoint[2] - current_setpoint[2];
// > Clip the difference to the maximum
if (diff_for_z > max_for_z)
diff_for_z = max_for_z;
else if (diff_for_z < -max_for_z)
diff_for_z = -max_for_z;
// > Update the current setpoint
current_setpoint[2] += diff_for_z;
// SMOOTH THE X-Y-COORIDINATES
// > Compute the max allowed change
......@@ -719,14 +712,19 @@ void smoothSetpointChanges( float target_setpoint[4] , float (&current_setpoint)
float diff_for_x = target_setpoint[0] - current_setpoint[0];
float diff_for_y = target_setpoint[1] - current_setpoint[1];
float diff_for_xy = sqrt( diff_for_x*diff_for_x + diff_for_y*diff_for_y );
// > Clip the difference to the maximum
if (diff_for_xy > max_for_xy)
// > Compute if outside the allowed ellipse
float ellipse_value = (diff_for_xy*diff_for_xy)/(max_for_xy*max_for_xy) + (diff_for_z*diff_for_z)/(max_for_z*max_for_z);
// > Clip the difference with outside the allowed ellispe
if (ellipse_value > 1.0f)
{
// > Convert the difference to a proportion
float proportion_xy = max_for_xy / diff_for_xy;
// > Compute the proportion
float proportion_xyz = 1.0f / sqrt(ellipse_value);
// > Update the current setpoint
current_setpoint[0] += proportion_xy * diff_for_x;
current_setpoint[1] += proportion_xy * diff_for_y;
current_setpoint[0] += proportion_xyz * diff_for_x;
current_setpoint[1] += proportion_xyz * diff_for_y;
current_setpoint[2] += proportion_xyz * diff_for_z;
}
else
{
......@@ -735,7 +733,9 @@ void smoothSetpointChanges( float target_setpoint[4] , float (&current_setpoint)
// reach
current_setpoint[0] = target_setpoint[0];
current_setpoint[1] = target_setpoint[1];
}
current_setpoint[2] = target_setpoint[2];
}
}
......@@ -1014,9 +1014,17 @@ void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controlle
- yaml_gainMatrixPitchAngle_2StateVector[1] * stateErrorBody[3];
// Clip the request to within the specified limits
if (pitchAngle_desired > yaml_max_roll_pitch_request_radians)
{
pitchAngle_desired = yaml_max_roll_pitch_request_radians;
//ROS_ERROR_STREAM("[DEFAULT CONTROLLER] pitchAngle_desired = " << pitchAngle_desired);
}
else if (pitchAngle_desired < -yaml_max_roll_pitch_request_radians)
pitchAngle_desired = -yaml_max_roll_pitch_request_radians;
{
pitchAngle_desired = -yaml_max_roll_pitch_request_radians;
//ROS_ERROR_STREAM("[DEFAULT CONTROLLER] pitchAngle_desired = " << pitchAngle_desired);
}
// > Compute the pitch rate
pitchRate_forResponse =
- yaml_gainPitchRate_fromAngle * (stateErrorBody[7] - pitchAngle_desired);
......
......@@ -116,6 +116,43 @@ void viconCallback(const ViconData& viconData)
)
{
// If motors-off and testing motors
if (
m_flying_state == STATE_MOTORS_OFF
&&
m_controller_nominally_selected == TESTMOTORS_CONTROLLER
&&
m_testMotorsController.exists()
)
{
// Initliase the "Contrller" service call variable
Controller testMotorsCall;
// Initialise a local boolean success variable
bool isSuccessful_testMotorsCall = false;
// Call the controller service client
isSuccessful_testMotorsCall = m_testMotorsController.call(testMotorsCall);
// Ensure success and enforce safety
if(isSuccessful_testMotorsCall)
{
m_commandForSendingToCrazyfliePublisher.publish(testMotorsCall.response.controlOutput);
}
else
{
// Let the user know that the controller call failed
ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to call test motors controller, valid: " << m_testMotorsController.isValid() << ", exists: " << m_testMotorsController.exists());
// Change back to the default controller
setControllerNominallySelected(DEFAULT_CONTROLLER);
// Send the command to turn the motors off
sendZeroOutputCommandForMotors();
}
return;
}
// Only continue if:
// (1) the agent is NOT occulded
if(!poseDataForThisAgent.occluded)
......@@ -726,6 +763,9 @@ void setInstantController(int controller)
case TEMPLATE_CONTROLLER:
m_instant_controller_service_client = &m_templateController;
break;
case TESTMOTORS_CONTROLLER:
m_instant_controller_service_client = &m_defaultController;
break;
default:
break;
}
......@@ -863,6 +903,7 @@ void flyingStateRequestCallback(const IntWithHeader & msg) {
setControllerNominallySelected(TEMPLATE_CONTROLLER);
break;
case CMD_CRAZYFLY_TAKE_OFF:
ROS_INFO("[FLYING AGENT CLIENT] TAKE_OFF Command received");
requestChangeFlyingStateTo(STATE_TAKE_OFF);
......@@ -877,6 +918,20 @@ void flyingStateRequestCallback(const IntWithHeader & msg) {
requestChangeFlyingStateTo(STATE_MOTORS_OFF);
break;
case CMD_USE_TESTMOTORS_CONTROLLER:
if (m_flying_state == STATE_MOTORS_OFF)
{
ROS_INFO("[FLYING AGENT CLIENT] USE_TEST_MOTORS_CONTROLLER Command received");
setControllerNominallySelected(TESTMOTORS_CONTROLLER);
}
else
{
ROS_INFO("[FLYING AGENT CLIENT] USE_TEST_MOTORS_CONTROLLER Command received, but state is not currently STATE_MOTORS_OFF");
}
break;
default:
ROS_ERROR_STREAM("[FLYING AGENT CLIENT] unexpected command number: " << cmd);
break;
......@@ -1141,6 +1196,8 @@ void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
createControllerServiceClientFromParameterName( "pickerController" , m_pickerController );
createControllerServiceClientFromParameterName( "templateController" , m_templateController );
createControllerServiceClientFromParameterName( "testMotorsController" , m_testMotorsController );
// INITIALISE THE SERVICE FOR REQUESTING THE DEFAULT
// CONTROLLER TO PERFORM MANOEUVRES
createIntIntServiceClientFromParameterName( "defaultController_requestManoeuvre" , m_defaultController_requestManoeuvre );
......
Markdown is supported
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