Commit 849e2387 authored by beuchatp's avatar beuchatp
Browse files

Added the Template Controller ROS node. Needs to be compiled and tested.

parent eac28fac
......@@ -117,6 +117,7 @@ set(SRC_HDRS_QOBJECT_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_INC}/rosNodeThread_for_flyingAgentGUI.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/safecontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/studentcontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/templatecontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/topbanner.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/tuningcontrollertab.h
)
......@@ -132,6 +133,7 @@ qt5_wrap_ui(UIS_HDRS_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/pickercontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/safecontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/studentcontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/templatecontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/topbanner.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/tuningcontrollertab.ui
)
......@@ -324,6 +326,8 @@ add_executable(TuningControllerService src/nodes/TuningControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(PickerControllerService src/nodes/PickerControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(TemplateControllerService src/nodes/TemplateControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(ParameterService src/nodes/ParameterService.cpp)
......@@ -365,6 +369,7 @@ set(FLYING_AGENT_GUI_CPP_SOURCES # compilation of sources
${FLYING_AGENT_GUI_LIB_PATH_SRC}/pickercontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/safecontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/studentcontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/templatecontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/topbanner.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/tuningcontrollertab.cpp
)
......@@ -397,6 +402,7 @@ add_dependencies(MpcControllerService dfall_pkg_generate_messages_cpp ${cat
add_dependencies(RemoteControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
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(CentralManagerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(ParameterService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
......@@ -448,6 +454,7 @@ endif()
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(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(ParameterService ${catkin_LIBRARIES})
......
......@@ -135,6 +135,8 @@ ros::ServiceClient remoteController;
ros::ServiceClient tuningController;
// The Picker controller specified in the ClientConfig.yaml
ros::ServiceClient pickerController;
// The Template controller specified in the ClientConfig.yaml
ros::ServiceClient templateController;
//values for safteyCheck
......@@ -289,6 +291,7 @@ void loadMpcController();
void loadRemoteController();
void loadTuningController();
void loadPickerController();
void loadTemplateController();
void sendMessageUsingController(int controller);
void setInstantController(int controller);
......
......@@ -213,10 +213,11 @@ float m_weight_cf_in_newtons = 0.0;
std::vector<float> m_previous_stateErrorInertial = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
std::vector<float> yaml_gainMatrixThrust_NineStateVector (9,0.0);
std::vector<float> yaml_gainMatrixRollRate (9,0.0);
std::vector<float> yaml_gainMatrixPitchRate (9,0.0);
std::vector<float> yaml_gainMatrixYawRate (9,0.0);
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 16-bit command limits
float yaml_cmd_sixteenbit_min = 1000;
......
// 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 Template Controller for students build from
//
// ----------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------
// 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
......@@ -110,6 +110,15 @@
>
</node>
<!-- TEMPLATE CONTROLLER -->
<node
pkg = "dfall_pkg"
name = "TemplateControllerService"
output = "screen"
type = "TemplateControllerService"
>
</node>
<!-- PARAMETER SERVICE -->
<node
pkg = "dfall_pkg"
......@@ -149,6 +158,11 @@
file = "$(find dfall_pkg)/param/PickerController.yaml"
ns = "PickerController"
/>
<rosparam
command = "load"
file = "$(find dfall_pkg)/param/TemplateController.yaml"
ns = "TemplateController"
/>
</node>
......
safeController: "SafeControllerService/RateController"
demoController: "DemoControllerService/DemoController"
studentController: "StudentControllerService/StudentController"
mpcController: "MpcControllerService/MpcController"
remoteController: "RemoteControllerService/RemoteController"
tuningController: "TuningControllerService/TuningController"
pickerController: "PickerControllerService/PickerController"
safeController: "SafeControllerService/RateController"
demoController: "DemoControllerService/DemoController"
studentController: "StudentControllerService/StudentController"
mpcController: "MpcControllerService/MpcController"
remoteController: "RemoteControllerService/RemoteController"
tuningController: "TuningControllerService/TuningController"
pickerController: "PickerControllerService/PickerController"
templateController: "TemplateControllerService/TemplateController"
# Flag for whether to use angle for switching to the Safe Controller
strictSafety: false
......@@ -12,5 +13,4 @@ angleMargin: 0.8
battery_threshold_while_flying: 2.60 # in V
battery_threshold_while_motors_off: 3.30 # in V
battery_polling_period: 200 # in ms
battery_polling_period: 200 # in ms
\ No newline at end of file
# Mass of the crazyflie
mass : 28
# Frequency of the controller, in hertz
control_frequency : 200
# Quadratic motor regression equation (a0, a1, a2)
motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
# The min and max for saturating 16 bit thrust commands
command_sixteenbit_min : 1000
command_sixteenbit_max : 60000
# The default setpoint, the ordering is (x,y,z,yaw),
# with unit [meters,meters,meters,radians]
default_setpoint : [0.0, 0.0, 0.4, 0.0]
# Boolean indiciating whether the "Debug Message" of this agent should be published or not
shouldPublishDebugMessage : false
# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
shouldDisplayDebugInfo : false
# The LQR Controller parameters for rate mode
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]
......@@ -975,11 +975,7 @@ int main(int argc, char* argv[])
// and the message received is passed as an input argument to the callback function.
ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("CustomButtonPressed", 1, customCommandReceivedCallback);
// Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
// to the name space of this node, i.e., "dfall_pkg" as specified by the line:
// "using namespace dfall_pkg;"
// in the "DEFINES" section at the top of this file.
//ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
// Print out some information to the user.
ROS_INFO("[DEFAULT CONTROLLER] Service ready :-)");
......
......@@ -307,6 +307,9 @@ void viconCallback(const ViconData& viconData)
case PICKER_CONTROLLER:
success = pickerController.call(controllerCall);
break;
case TEMPLATE_CONTROLLER:
success = templateController.call(controllerCall);
break;
default:
ROS_ERROR("[FLYING AGENT CLIENT] the current controller was not recognised");
break;
......@@ -482,6 +485,11 @@ void commandCallback(const IntWithHeader & msg) {
setControllerUsed(PICKER_CONTROLLER);
break;
case CMD_USE_TEMPLATE_CONTROLLER:
ROS_INFO("[FLYING AGENT CLIENT] USE_TEMPLATE_CONTROLLER Command received");
setControllerUsed(TEMPLATE_CONTROLLER);
break;
case CMD_CRAZYFLY_TAKE_OFF:
ROS_INFO("[FLYING AGENT CLIENT] TAKE_OFF Command received");
if(flying_state == STATE_MOTORS_OFF)
......@@ -708,6 +716,23 @@ void loadPickerController()
ROS_INFO_STREAM("[FLYING AGENT CLIENT] Loaded picker controller " << pickerController.getService());
}
void loadTemplateController()
{
//ros::NodeHandle nodeHandle("~");
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");
std::string templateControllerName;
if(!nodeHandle.getParam("templateController", templateControllerName))
{
ROS_ERROR("[FLYING AGENT CLIENT] Failed to get template controller name");
return;
}
templateController = ros::service::createClient<Controller>(templateControllerName, true);
ROS_INFO_STREAM("[FLYING AGENT CLIENT] Loaded template controller " << templateController.getService());
}
void sendMessageUsingController(int controller)
{
// Send a message on the topic for informing the Flying
......@@ -744,6 +769,9 @@ void setInstantController(int controller) //for right now, temporal use
case PICKER_CONTROLLER:
loadPickerController();
break;
case TEMPLATE_CONTROLLER:
loadTemplateController();
break;
default:
break;
}
......
......@@ -1641,11 +1641,7 @@ int main(int argc, char* argv[]) {
// of this service the "calculateControlOutput" function is called.
ros::ServiceServer service = nodeHandle.advertiseService("RemoteController", calculateControlOutput);
// Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
// to the name space of this node, i.e., "dfall_pkg" as specified by the line:
// "using namespace dfall_pkg;"
// in the "DEFINES" section at the top of this file.
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
// Print out some information to the user.
ROS_INFO("[REMOTE CONTROLLER] Service ready :-)");
......
......@@ -342,8 +342,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// PREPARE AND RETURN THE VARIABLE "response"
/*choosing the Crazyflie onBoard controller type.
it can either be Motor, Rate or Angle based */
// Choose the controller type use on-board the Crazyflie,
// it can be either be Motor, Rate, or Angle based
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
......@@ -685,7 +685,7 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
default:
{
// Let the user know that the command was not recognised
ROS_INFO_STREAM("[DEMO CONTROLLER] A button clicked command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.float_data = " << float_data );
ROS_INFO_STREAM("[STUDENT CONTROLLER] A button clicked command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.float_data = " << float_data );
break;
}
}
......@@ -1039,11 +1039,7 @@ int main(int argc, char* argv[]) {
// And now we can instantiate the subscriber:
ros::Subscriber customCommandReceivedSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("StudentControllerService/CustomButtonPressed", 1, customCommandReceivedCallback);
// Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
// to the name space of this node, i.e., "dfall_pkg" as specified by the line:
// "using namespace dfall_pkg;"
// in the "DEFINES" section at the top of this file.
//ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
// Print out some information to the user.
ROS_INFO("[STUDENT CONTROLLER] Service ready :-)");
......
This diff is collapsed.
......@@ -1774,14 +1774,6 @@ int main(int argc, char* argv[]) {
// Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
// to the name space of this node, i.e., "dfall_pkg" as specified by the line:
// "using namespace dfall_pkg;"
// in the "DEFINES" section at the top of this file.
//ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
// Print out some information to the user.
ROS_INFO("[TUNING CONTROLLER] Service ready :-)");
......
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