Commit a4a8bda8 authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Connected all four controller (safe,demo,student,mpc) to the parameter service

parent d68e00e4
......@@ -58,6 +58,9 @@
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomButton.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
#include <std_msgs/Int32.h>
......@@ -145,12 +148,6 @@
#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
// Constants for feching the yaml paramters
//#define FETCH_YAML_SAFE_CONTROLLER_AGENT 1
#define FETCH_YAML_DEMO_CONTROLLER_AGENT 2
//#define FETCH_YAML_SAFE_CONTROLLER_COORDINATOR 3
#define FETCH_YAML_DEMO_CONTROLLER_COORDINATOR 4
// Namespacing the package
using namespace d_fall_pps;
......
......@@ -57,6 +57,9 @@
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
// Need for having a ROS "bag" to store data for post-analysis
//#include <rosbag/bag.h>
......@@ -114,12 +117,6 @@
#define CONNECTING 1
#define DISCONNECTED 2
// For which controller parameters to load
#define FETCH_YAML_SAFE_CONTROLLER_AGENT 1
#define FETCH_YAML_DEMO_CONTROLLER_AGENT 2
#define FETCH_YAML_SAFE_CONTROLLER_COORDINATOR 3
#define FETCH_YAML_DEMO_CONTROLLER_COORDINATOR 4
// Universal constants
#define PI 3.141592653589
......
......@@ -53,6 +53,9 @@
#include "d_fall_pps/Controller.h"
// Include the shared definitions
#include "nodes/ParameterServiceDefinitions.h"
// ----------------------------------------------------------------------------------
// DDDD EEEEE FFFFF III N N EEEEE SSSS
......@@ -62,35 +65,6 @@
// DDDD EEEEE F III N N EEEEE SSSS
// ----------------------------------------------------------------------------------
// For which controller parameters to load from file
#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
#define LOAD_YAML_DEMO_CONTROLLER_AGENT 2
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 11
#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR 12
#define LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR 13
#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR 14
// For send commends to the controller node informing which
// parameters to fetch
// > NOTE: these are identical to the #defines above, but
// used because thez have the same name as used in
// the controller files
// #define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
// #define FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT 2
// #define FETCH_YAML_STUDENT_CONTROLLER_FROM_OWN_AGENT 3
// #define FETCH_YAML_MPC_CONTROLLER_FROM_OWN_AGENT 4
// #define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 11
// #define FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR 12
// #define FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR 13
// #define FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR 14
// The types, i.e., agent or coordinator
#define TYPE_INVALID -1
#define TYPE_COORDINATOR 1
......
// 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:
// The service that manages the loading of YAML parameters
//
// ----------------------------------------------------------------------------------
// DEFINES THAT ARE SHARED WITH OTHER FILES
// For which controller parameters to load from file
#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
#define LOAD_YAML_DEMO_CONTROLLER_AGENT 2
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 11
#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR 12
#define LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR 13
#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR 14
// For sending commands to the controller node informing
// which parameters to fetch
// > NOTE: these are identical to the #defines above, but
// used because they have the name distinguishes
// between:
// - "loading" a yaml file into ram
// - "fetching" the values that were loaded into ram
#define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
#define FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT 2
#define FETCH_YAML_STUDENT_CONTROLLER_FROM_OWN_AGENT 3
#define FETCH_YAML_MPC_CONTROLLER_FROM_OWN_AGENT 4
#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 11
#define FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR 12
#define FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR 13
#define FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR 14
\ No newline at end of file
// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Cyrill Burgener, Marco Mueller, Philipp Friedli
//
// 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:
// The fall-back controller that keeps the Crazyflie safe
//
// ----------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------
// 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.
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
#include <std_msgs/String.h>
#include <ros/package.h>
#include "std_msgs/Float32.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include <std_msgs/Int32.h>
// Include the shared definitions
#include "nodes/ParameterServiceDefinitions.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
// The 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 MOTOR_MODE 6
#define RATE_MODE 7
#define ANGLE_MODE 8
// Namespacing the package
using namespace d_fall_pps;
// ----------------------------------------------------------------------------------
// 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
// ----------------------------------------------------------------------------------
std::vector<float> ffThrust(4);
std::vector<float> feedforwardMotor(4);
float cf_mass;
float gravity_force;
std::vector<float> motorPoly(3);
std::vector<float> gainMatrixRoll(9);
std::vector<float> gainMatrixPitch(9);
std::vector<float> gainMatrixYaw(9);
std::vector<float> gainMatrixThrust(9);
//K_infinite of feedback
std::vector<float> filterGain(6);
//only for velocity calculation
std::vector<float> estimatorMatrix(2);
float prevEstimate[9];
std::vector<float> setpoint(4);
std::vector<float> defaultSetpoint(4);
float saturationThrust;
CrazyflieData previousLocation;
// Variable for the namespaces for the paramter 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;
// ----------------------------------------------------------------------------------
// 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.
// > For the CONTROL LOOP
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured);
float computeMotorPolyBackward(float thrust);
void estimateState(Controller::Request &request, float (&est)[9]);
// > For the LOAD PARAMETERS
void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
void fetchYamlParameters(ros::NodeHandle& nodeHandle);
void processFetchedParameters();
// > For the GETPARAM()
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);
\ No newline at end of file
......@@ -58,6 +58,9 @@
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomControllerYAML.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
#include <std_msgs/Int32.h>
......@@ -107,12 +110,6 @@
#define LQR_RATE_MODE 1 // (DEFAULT)
#define LQR_ANGLE_MODE 2
// Constants for feching the yaml paramters
#define FETCH_YAML_SAFE_CONTROLLER_AGENT 1
#define FETCH_YAML_CUSTOM_CONTROLLER_AGENT 2
#define FETCH_YAML_SAFE_CONTROLLER_COORDINATOR 3
#define FETCH_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
// Namespacing the package
using namespace d_fall_pps;
......
......@@ -47,6 +47,24 @@
>
</node>
<!-- STUDENT CONTROLLER -->
<node
pkg = "d_fall_pps"
name = "StudentControllerService"
output = "screen"
type = "StudentControllerService"
>
</node>
<!-- MPC CONTROLLER -->
<node
pkg = "d_fall_pps"
name = "MpcControllerService"
output = "screen"
type = "MpcControllerService"
>
</node>
<!-- PARAMETER SERVICE -->
<node
pkg = "d_fall_pps"
......
# Mass of the crazyflie
mass : 30
mass : 29
# Frequency of the controller, in hertz
vicon_frequency : 200
......
# Mass of the crazyflie
mass : 31
mass : 27
# Frequency of the controller, in hertz
control_frequency : 200
......
# Mass of the crazyflie
mass : 31
mass : 28
# Frequency of the controller, in hertz
control_frequency : 200
......
......@@ -267,7 +267,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
default:
{
// Display that the "estimator_method" was not recognised
ROS_INFO_STREAM("ERROR: in the 'calculateControlOutput' function of the 'DemoControllerService': the 'estimator_method' is not recognised.");
ROS_INFO_STREAM("[DEMO CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DemoControllerService': the 'estimator_method' is not recognised.");
// Transfer the finite difference estimate by default
for(int i = 0; i < 12; ++i)
{
......@@ -450,7 +450,7 @@ void calculateControlOutput_viaLQR(float stateErrorBody[12], Controller::Request
default:
{
// Display that the "controller_mode" was not recognised
ROS_INFO_STREAM("ERROR: in the 'calculateControlOutput' function of the 'DemoControllerService': the 'controller_mode' is not recognised.");
ROS_INFO_STREAM("[DEMO CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DemoControllerService': the 'controller_mode' is not recognised.");
// Set everything in the response to zero
response.controlOutput.roll = 0;
response.controlOutput.pitch = 0;
......@@ -1356,7 +1356,7 @@ void customCommandReceivedCallback(const CustomButton& commandReceived)
case 1:
{
// Let the user know that this part of the code was triggered
ROS_INFO("Custom button 1 received in controller.");
ROS_INFO("[DEMO CONTROLLER] Custom button 1 received in controller.");
// Code here to respond to custom button 1
break;
......@@ -1366,7 +1366,7 @@ void customCommandReceivedCallback(const CustomButton& commandReceived)
case 2:
{
// Let the user know that this part of the code was triggered
ROS_INFO("Custom button 2 received in controller.");
ROS_INFO("[DEMO CONTROLLER] Custom button 2 received in controller.");
// Code here to respond to custom button 2
break;
......@@ -1376,7 +1376,7 @@ void customCommandReceivedCallback(const CustomButton& commandReceived)
case 3:
{
// Let the user know that this part of the code was triggered
ROS_INFO_STREAM("Custom button 3 received in controller, with command code:" << custom_command_code );
ROS_INFO_STREAM("[DEMO CONTROLLER] Custom button 3 received in controller, with command code:" << custom_command_code );
// Code here to respond to custom button 3
break;
......@@ -1385,7 +1385,7 @@ void customCommandReceivedCallback(const CustomButton& commandReceived)
default:
{
// Let the user know that the command was not recognised
ROS_INFO_STREAM("A custom command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.command_code = " << custom_command_code );
ROS_INFO_STREAM("[DEMO CONTROLLER] A custom command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.command_code = " << custom_command_code );
break;
}
}
......@@ -1445,10 +1445,10 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
{
// > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
case FETCH_YAML_DEMO_CONTROLLER_AGENT:
case FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT:
{
// Let the user know that this message was received
ROS_INFO("The DemoControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
ROS_INFO("[DEMO CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
// Create a node handle to the parameter service running on this agent's machine
ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
// Call the function that fetches the parameters
......@@ -1457,10 +1457,10 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
}
// > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
case FETCH_YAML_DEMO_CONTROLLER_COORDINATOR:
case FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR:
{
// Let the user know that this message was received
ROS_INFO("The DemoControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
ROS_INFO("[DEMO CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
// Create a node handle to the parameter service running on the coordinator machine
ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
// Call the function that fetches the parameters
......@@ -1596,8 +1596,8 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("DEBUGGING: the fetched DemoController/mass = " << cf_mass);
ROS_INFO_STREAM("DEBUGGING: the fetched DemoController/angleResponseTest_pitchAngle_degrees = " << angleResponseTest_pitchAngle_degrees);
ROS_INFO_STREAM("[DEMO CONTROLLER] DEBUGGING: the fetched DemoController/mass = " << cf_mass);
ROS_INFO_STREAM("[DEMO CONTROLLER] DEBUGGING: the fetched DemoController/angleResponseTest_pitchAngle_degrees = " << angleResponseTest_pitchAngle_degrees);
// Call the function that computes details an values that are needed from these
// parameters loaded above
......@@ -1692,7 +1692,7 @@ void processFetchedParameters()
if (shouldFollowAnotherAgent)
{
ROS_INFO_STREAM("This agent (with ID " << my_agentID << ") will now follow agent ID " << agentID_to_follow );
ROS_INFO_STREAM("[DEMO CONTROLLER] This agent (with ID " << my_agentID << ") will now follow agent ID " << agentID_to_follow );
}
}
......@@ -1792,7 +1792,7 @@ int main(int argc, char* argv[]) {
// Get the namespace of this "DemoControllerService" node
std::string m_namespace = ros::this_node::getNamespace();
ROS_INFO_STREAM("For DemoController, ros::this_node::getNamespace() = " << m_namespace);
ROS_INFO_STREAM("[DEMO CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
// Get the agent ID as the "ROS_NAMESPACE" this computer.
// NOTES:
......@@ -1809,7 +1809,7 @@ int main(int argc, char* argv[]) {
if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
{
// Throw an error if the student ID parameter could not be obtained
ROS_ERROR("Failed to get agentID from PPSClient");
ROS_ERROR("[DEMO CONTROLLER] Failed to get agentID from PPSClient");
}
......@@ -1860,9 +1860,9 @@ int main(int argc, char* argv[]) {
// PRINT OUT SOME INFORMATION
// Let the user know what namespaces are being used for linking to the parameter service
ROS_INFO_STREAM("For DemoController: the namespace strings for accessing the Paramter Services are:");
ROS_INFO_STREAM("namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
ROS_INFO_STREAM("namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
ROS_INFO_STREAM("[DEMO CONTROLLER] the namespace strings for accessing the Paramter Services are:");
ROS_INFO_STREAM("[DEMO CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
ROS_INFO_STREAM("[DEMO CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
// FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
......@@ -1914,7 +1914,7 @@ int main(int argc, char* argv[]) {
ros::Subscriber customCommandReceivedSubscriber = PPSClient_nodeHandle.subscribe("StudentCustomButton", 1, customCommandReceivedCallback);
// Print out some information to the user.
ROS_INFO("DemoControllerService ready");
ROS_INFO("[DEMO CONTROLLER] Service ready :-)");
// Enter an endless while loop to keep the node alive.
ros::spin();
......
......@@ -58,6 +58,9 @@
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomControllerYAML.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
#include <std_msgs/Int32.h>
......@@ -107,12 +110,6 @@
#define LQR_RATE_MODE 1 // (DEFAULT)
#define LQR_ANGLE_MODE 2
// Constants for feching the yaml paramters
#define FETCH_YAML_SAFE_CONTROLLER_AGENT 1
#define FETCH_YAML_CUSTOM_CONTROLLER_AGENT 2
#define FETCH_YAML_SAFE_CONTROLLER_COORDINATOR 3
#define FETCH_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
// Namespacing the package
using namespace d_fall_pps;
......@@ -739,10 +736,10 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
{
// > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
case FETCH_YAML_CUSTOM_CONTROLLER_AGENT:
case FETCH_YAML_MPC_CONTROLLER_FROM_OWN_AGENT:
{
// Let the user know that this message was received
ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
ROS_INFO("[MPC CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
// Create a node handle to the parameter service running on this agent's machine
ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
// Call the function that fetches the parameters
......@@ -751,10 +748,10 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
}
// > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
case FETCH_YAML_CUSTOM_CONTROLLER_COORDINATOR:
case FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR:
{
// Let the user know that this message was received
ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
ROS_INFO("[MPC CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
// Create a node handle to the parameter service running on the coordinator machine
ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
// Call the function that fetches the parameters
......@@ -781,7 +778,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// Here we load the parameters that are specified in the CustomController.yaml file
// Add the "CustomController" namespace to the "nodeHandle"
ros::NodeHandle nodeHandle_for_customController(nodeHandle, "CustomController");
ros::NodeHandle nodeHandle_for_customController(nodeHandle, "MpcController");
// > The mass of the crazyflie
cf_mass = getParameterFloat(nodeHandle_for_customController , "mass");
......@@ -799,7 +796,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("DEBUGGING: the fetched CustomController/mass = " << cf_mass);
ROS_INFO_STREAM("[MPC CONTROLLER] DEBUGGING: the fetched CustomController/mass = " << cf_mass);