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 78e55fe4 authored by beuchatp's avatar beuchatp
Browse files

Merge branch 'pre_fs2019_updates' into 'master'

Merge of pre-Spring 2019 semester updates into the master

See merge request D-FaLL/PandS-System/D-FaLL-System!12
parents 7cc3f5e1 0c468ec5
......@@ -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})
......
......@@ -215,6 +215,8 @@
<addaction name="action_showHideController_picker"/>
<addaction name="action_showHideController_tuning"/>
<addaction name="action_showHideController_template"/>
<addaction name="separator"/>
<addaction name="action_testMotors"/>
</widget>
<addaction name="menuFile"/>
<addaction name="menuLoad_YAML"/>
......@@ -313,6 +315,11 @@
<string>Template</string>
</property>
</action>
<action name="action_testMotors">
<property name="text">
<string>TestMotors</string>
</property>
</action>
</widget>
<layoutdefault spacing="6" margin="11"/>
<customwidgets>
......
......@@ -116,6 +116,8 @@
#define PICKER_CONTROLLER 7
#define TEMPLATE_CONTROLLER 8
#define TESTMOTORS_CONTROLLER 21
// The constants that "command" changes in the
// operation state of this agent
#define CMD_USE_DEFAULT_CONTROLLER 1
......@@ -132,6 +134,8 @@
#define CMD_CRAZYFLY_LAND 12
#define CMD_CRAZYFLY_MOTORS_OFF 13
#define CMD_USE_TESTMOTORS_CONTROLLER 21
// Flying states
#define STATE_MOTORS_OFF 1
#define STATE_TAKE_OFF 2
......@@ -208,4 +212,4 @@
// ----------------------------------------------------------------------------------
// For standard buttons in the GUI
#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
\ No newline at end of file
#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
......@@ -94,6 +94,8 @@ public:
void showHideController_tuning_changed();
void showHideController_template_changed();
void testMotors_triggered();
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
......
......@@ -133,6 +133,8 @@ private slots:
void on_action_showHideController_tuning_changed();
void on_action_showHideController_template_changed();
void on_action_testMotors_triggered();
};
#endif // MAINWINDOW_H
......@@ -116,6 +116,20 @@ void EnableControllerLoadYamlBar::showHideController_template_changed()
// TEST MOTORS
void EnableControllerLoadYamlBar::testMotors_triggered()
{
#ifdef CATKIN_MAKE
dfall_pkg::IntWithHeader msg;
fillIntMessageHeader(msg);
msg.data = CMD_USE_TESTMOTORS_CONTROLLER;
this->commandPublisher.publish(msg);
ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Test Motors Controller");
#endif
}
// ENABLE CONTROLLER BUTTONS ON-CLICK CALLBACK
......
......@@ -253,6 +253,12 @@ void MainWindow::on_action_showHideController_template_changed()
}
void MainWindow::on_action_testMotors_triggered()
{
// Notify the UI elements of this change
ui->customWidget_enableControllerLoadYamlBar->testMotors_triggered();
}
// ----------------------------------------------------------------------------------
// III DDDD &&& TTTTT Y Y PPPP EEEEE
......
......@@ -410,16 +410,27 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
QString filename(":/images/drone_fixed_");
if(std::regex_search(s, m, e))
{
std::string found_string = m[1].str();
filename.append(QString::fromStdString(found_string));
filename.append(".svg");
}
else
{
filename.append("unk.svg");
}
if(std::regex_search(s, m, e))
{
// Get the string that was found
// > it should be a zero-padded 2-digit number
std::string found_string = m[1].str();
// Convert to an integer
int found_string_as_int = QString::fromStdString(found_string).toInt();
if ((1 <= found_string_as_int) && (found_string_as_int <= 9))
{
filename.append(QString::fromStdString(found_string));
filename.append(".svg");
}
else
{
filename.append("unk.svg");
}
}
else
{
filename.append("unk.svg");
}
crazyFly* tmp_p_crazyfly = new crazyFly(&(p_msg->crazyflies[i]), filename);
tmp_p_crazyfly->setScaleCFs(ui->scaleSpinBox->value());
......
......@@ -129,6 +129,8 @@
#define PICKER_CONTROLLER 7
#define TEMPLATE_CONTROLLER 8
#define TESTMOTORS_CONTROLLER 21
// The constants that "command" changes in the
// operation state of this agent
#define CMD_USE_DEFAULT_CONTROLLER 1
......@@ -145,6 +147,8 @@
#define CMD_CRAZYFLY_LAND 12
#define CMD_CRAZYFLY_MOTORS_OFF 13
#define CMD_USE_TESTMOTORS_CONTROLLER 21
// Flying states
#define STATE_MOTORS_OFF 1
#define STATE_TAKE_OFF 2
......
......@@ -39,6 +39,14 @@
#define DEFAULT_CONTROLLER_REQUEST_LANDING 2
// TO NOTIFY THAT MANOEUVRES ARE COMPLETED
#define DEFAULT_CONTROLLER_TAKEOFF_COMPLETE 1
#define DEFAULT_CONTROLLER_LANDING_COMPLETE 2
// TO IDENITFY THE STATE OF THE DEFAULT CONTROLLER
#define DEFAULT_CONTROLLER_STATE_NORMAL 1
......
......@@ -70,6 +70,7 @@
#include "dfall_pkg/IntIntService.h"
#include "dfall_pkg/LoadYamlFromFilename.h"
#include "dfall_pkg/GetSetpointService.h"
#include "dfall_pkg/CMQuery.h"
// Include the shared definitions
#include "nodes/Constants.h"
......@@ -369,12 +370,27 @@ ros::Publisher m_debugPublisher;
// changes to the setpoint
ros::Publisher m_setpointChangedPublisher;
// ROS Publisher to inform the flying agent client
// when a requested manoeuvre is complete
ros::Publisher m_manoeuvreCompletePublisher;
// ROS Publisher for sending motors-off command
// to the flying agent client
ros::Publisher m_motorsOffToFlyingAgentClientPublisher;
// Describes the area of the crazyflie and other parameters
//CrazyflieContext m_context;
// Flag for whether a context is available or not
bool m_isAvailableContext = false;
// Variable for each coordinate
float m_symmetric_area_bounds_x = 0.5;
float m_symmetric_area_bounds_y = 0.5;
float m_symmetric_area_bounds_z = 0.5;
// Service Client from which the "CrazyflieContext" is loaded
ros::ServiceClient m_centralManager;
......@@ -458,12 +474,21 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
// PUBLISH THE CURRENT SETPOINT AND STATE
void publishCurrentSetpointAndState();
// CLIP TO MIN AND MAX
float clipToBounds(float val, float val_min, float val_max);
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
// PUBLISH MOTORS-OFF MESSAGE TO FLYING AGENT CLIENT
void publish_motors_off_to_flying_agent_client();
// FUNCTIONS FOR THE CONTEXT OF THIS AGENT
// > Callback that the context database changed
void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
// > For loading the context of this agent
void loadCrazyflieContext();
// LOADING OF YAML PARAMETERS
void timerCallback_initial_load_yaml(const ros::TimerEvent&);
void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg);
......
......@@ -204,6 +204,8 @@ ros::ServiceClient m_pickerController;
// The Template controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_templateController;
// The Test Mtoros controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_testMotorsController;
......@@ -294,6 +296,8 @@ void requestChangeFlyingStateToLand();
void takeOffTimerCallback(const ros::TimerEvent&);
// > Callback that the landing phase is complete
void landTimerCallback(const ros::TimerEvent&);
// > Callback that the Default Controller complete a manoeuvre
void defaultControllerManoeuvreCompleteCallback(const IntWithHeader & msg);
......
// 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: 4.0
# Height change for the landing move-down
......@@ -99,16 +99,16 @@ gainMatrixThrust_2StateVector : [ 0.98, 0.25]
gainMatrixRollRate_3StateVector : [-6.20,-3.00, 5.20]
gainMatrixPitchRate_3StateVector : [ 6.20, 3.00, 5.20]
# The LQR Controller parameters for mode 2 (Angle-nested)
gainMatrixRollAngle_2StateVector : [-0.20,-0.20]
gainMatrixPitchAngle_2StateVector : [ 0.20, 0.20]
gainMatrixRollAngle_2StateVector : [-0.80,-0.50]
gainMatrixPitchAngle_2StateVector : [ 0.80, 0.50]
gainRollRate_fromAngle : 4.00
gainPitchRate_fromAngle : 4.00
# The LQR Controller parameters for yaw
gainYawRate_fromAngle : 2.30
# Integrator gains
integratorGain_forThrust : 0.00
integratorGain_forTauXY : 0.00
integratorGain_forTauYaw : 0.00
integratorGain_forThrust : 0.10
integratorGain_forTauXY : 0.06
integratorGain_forTauYaw : 0.05
......
......@@ -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"
......
......@@ -538,12 +538,16 @@ void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
if (m_time_in_seconds > yaml_takoff_integrator_on_time)
{
// Inform the user
ROS_INFO("[DEFAULT CONTROLLER] Switch to state: normal");
ROS_INFO("[DEFAULT CONTROLLER] Publish message that take-off is complete, and switch to state: normal");
// Reset the time variable
m_time_in_seconds = 0.0;
// Update the state accordingly
m_current_state = DEFAULT_CONTROLLER_STATE_NORMAL;
m_current_state_changed = true;
// Publish a message that the take-off is complete
IntWithHeader msg;
msg.data = DEFAULT_CONTROLLER_TAKEOFF_COMPLETE;
m_manoeuvreCompletePublisher.publish(msg);
}
}
......@@ -664,12 +668,16 @@ void computeResponse_for_landing_spin_motors(Controller::Response &response)
if ( m_time_in_seconds > (0.7*yaml_landing_spin_motors_time) )
{
// Inform the user
ROS_INFO("[DEFAULT CONTROLLER] Switch to state: standby");
ROS_INFO("[DEFAULT CONTROLLER] Publish message that landing is complete, and switch to state: standby");