From a4a8bda8a8e2bc9a520b6286268f7e91d5e0f580 Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Wed, 25 Apr 2018 09:04:44 +0200
Subject: [PATCH] Connected all four controller (safe,demo,student,mpc) to the
parameter service
---
.../include/nodes/DemoControllerService.h | 9 +-
.../src/d_fall_pps/include/nodes/PPSClient.h | 9 +-
.../include/nodes/ParameterService.h | 32 +---
.../nodes/ParameterServiceDefinitions.h | 64 +++++++
.../include/nodes/SafeControllerService.h | 179 ++++++++++++++++++
.../include/nodes/StudentControllerService.h | 9 +-
pps_ws/src/d_fall_pps/launch/Agent.launch | 18 ++
.../src/d_fall_pps/param/DemoController.yaml | 2 +-
.../src/d_fall_pps/param/MpcController.yaml | 2 +-
.../d_fall_pps/param/StudentController.yaml | 2 +-
.../src/nodes/DemoControllerService.cpp | 38 ++--
.../src/nodes/MpcControllerService.cpp | 43 ++---
pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp | 104 +++++-----
.../d_fall_pps/src/nodes/ParameterService.cpp | 32 ++--
.../src/nodes/SafeControllerService.cpp | 166 ++--------------
.../src/nodes/StudentControllerService.cpp | 34 ++--
16 files changed, 413 insertions(+), 330 deletions(-)
create mode 100644 pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
create mode 100644 pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
diff --git a/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
index 765cc803..7c0f469f 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
@@ -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;
diff --git a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
index 76597786..46a65d04 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
@@ -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
diff --git a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h b/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
index e4d82bbe..6197ce6d 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
@@ -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
diff --git a/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h b/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
new file mode 100644
index 00000000..17d3b1dc
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
@@ -0,0 +1,64 @@
+// 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
diff --git a/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
new file mode 100644
index 00000000..02e32fa0
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
@@ -0,0 +1,179 @@
+// 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
diff --git a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
index 7e01a077..a95e3074 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
@@ -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;
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
index fbee9e65..6f546763 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -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"
diff --git a/pps_ws/src/d_fall_pps/param/DemoController.yaml b/pps_ws/src/d_fall_pps/param/DemoController.yaml
index f16f8f34..3328dd54 100644
--- a/pps_ws/src/d_fall_pps/param/DemoController.yaml
+++ b/pps_ws/src/d_fall_pps/param/DemoController.yaml
@@ -1,5 +1,5 @@
# Mass of the crazyflie
-mass : 30
+mass : 29
# Frequency of the controller, in hertz
vicon_frequency : 200
diff --git a/pps_ws/src/d_fall_pps/param/MpcController.yaml b/pps_ws/src/d_fall_pps/param/MpcController.yaml
index 69c089c9..8a133950 100644
--- a/pps_ws/src/d_fall_pps/param/MpcController.yaml
+++ b/pps_ws/src/d_fall_pps/param/MpcController.yaml
@@ -1,5 +1,5 @@
# Mass of the crazyflie
-mass : 31
+mass : 27
# Frequency of the controller, in hertz
control_frequency : 200
diff --git a/pps_ws/src/d_fall_pps/param/StudentController.yaml b/pps_ws/src/d_fall_pps/param/StudentController.yaml
index 69c089c9..7dbb3ec7 100644
--- a/pps_ws/src/d_fall_pps/param/StudentController.yaml
+++ b/pps_ws/src/d_fall_pps/param/StudentController.yaml
@@ -1,5 +1,5 @@
# Mass of the crazyflie
-mass : 31
+mass : 28
# Frequency of the controller, in hertz
control_frequency : 200
diff --git a/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
index 9f379f67..d734b55a 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
@@ -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();
diff --git a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
index 194d25e5..f2aa6049 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
@@ -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);
// Call the function that computes details an values that are needed from these
// parameters loaded above
@@ -818,7 +815,7 @@ void processFetchedParameters()
gravity_force = cf_mass * 9.81/(1000*4);
// DEBUGGING: Print out one of the computed quantities
- ROS_INFO_STREAM("DEBUGGING: thus the graity force = " << gravity_force);
+ ROS_INFO_STREAM("[MPC CONTROLLER] DEBUGGING: thus the graity force = " << gravity_force);
}
@@ -908,12 +905,16 @@ bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
int main(int argc, char* argv[]) {
// Starting the ROS-node
- ros::init(argc, argv, "CustomControllerService");
+ ros::init(argc, argv, "MpcControllerService");
// Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
// the "~" indcates that "self" is the node handle assigned to this variable.
ros::NodeHandle nodeHandle("~");
+ // Get the namespace of this "StudentControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[MPC CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
// Get the agent ID as the "ROS_NAMESPACE" this computer.
// NOTES:
// > If you look at the "Student.launch" file in the "launch" folder, you will see
@@ -922,15 +923,13 @@ int main(int argc, char* argv[]) {
// This line of code adds a parameter named "studentID" to the "PPSClient"
// > Thus, to get access to this "studentID" paremeter, we first need to get a handle
// to the "PPSClient" node within which this controller service is nested.
- // Get the namespace of this "CustomControllerService" node
- std::string m_namespace = ros::this_node::getNamespace();
// Get the handle to the "PPSClient" node
ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
// Get the value of the "studentID" parameter into the instance variable "my_agentID"
- if(!PPSClient_nodeHandle.getParam("studentID", my_agentID))
+ if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
{
// Throw an error if the student ID parameter could not be obtained
- ROS_ERROR("Failed to get studentID from FollowN_1Service");
+ ROS_ERROR("[MPC CONTROLLER] Failed to get agentID from PPSClient");
}
@@ -981,9 +980,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("The namespace string 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("[MPC CONTROLLER] The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("[MPC CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[MPC CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
// FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
@@ -1023,7 +1022,7 @@ int main(int argc, char* argv[]) {
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
// Print out some information to the user.
- ROS_INFO("CustomControllerService ready");
+ ROS_INFO("[MPC CONTROLLER] Service ready :-)");
// Enter an endless while loop to keep the node alive.
ros::spin();
diff --git a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
index a55b808e..ef718af6 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
@@ -65,15 +65,15 @@
bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
//position check
if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
- ROS_INFO_STREAM("x safety failed");
+ ROS_INFO_STREAM("[PPS CLIENT] x safety failed");
return false;
}
if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
- ROS_INFO_STREAM("y safety failed");
+ ROS_INFO_STREAM("[PPS CLIENT] y safety failed");
return false;
}
if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) {
- ROS_INFO_STREAM("z safety failed");
+ ROS_INFO_STREAM("[PPS CLIENT] z safety failed");
return false;
}
@@ -82,11 +82,11 @@ bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
//the angleMargin is a value in the range (0,1). The closer to 1, the closer to 90 deg are the roll and pitch angles allowed to become before the safeController takes over
if(strictSafety){
if((data.roll > PI/2*angleMargin) or (data.roll < -PI/2*angleMargin)) {
- ROS_INFO_STREAM("roll too big.");
+ ROS_INFO_STREAM("[PPS CLIENT] roll too big.");
return false;
}
if((data.pitch > PI/2*angleMargin) or (data.pitch < -PI/2*angleMargin)) {
- ROS_INFO_STREAM("pitch too big.");
+ ROS_INFO_STREAM("[PPS CLIENT] pitch too big.");
return false;
}
}
@@ -137,11 +137,11 @@ void takeOffCF(CrazyflieData& current_local_coordinates) //local because the set
// setpoint_msg.yaw = current_local_coordinates.yaw; //previous one
setpoint_msg.yaw = 0.0;
safeControllerServiceSetpointPublisher.publish(setpoint_msg);
- ROS_INFO("Take OFF:");
- ROS_INFO("------Current coordinates:");
- ROS_INFO("X: %f, Y: %f, Z: %f", current_local_coordinates.x, current_local_coordinates.y, current_local_coordinates.z);
- ROS_INFO("------New coordinates:");
- ROS_INFO("X: %f, Y: %f, Z: %f", setpoint_msg.x, setpoint_msg.y, setpoint_msg.z);
+ ROS_INFO("[PPS CLIENT] Take OFF:");
+ ROS_INFO("[PPS CLIENT] ------Current coordinates:");
+ ROS_INFO("[PPS CLIENT] X: %f, Y: %f, Z: %f", current_local_coordinates.x, current_local_coordinates.y, current_local_coordinates.z);
+ ROS_INFO("[PPS CLIENT] ------New coordinates:");
+ ROS_INFO("[PPS CLIENT] X: %f, Y: %f, Z: %f", setpoint_msg.x, setpoint_msg.y, setpoint_msg.z);
// now, use safe controller to go to that setpoint
loadSafeController();
@@ -172,12 +172,12 @@ void changeFlyingStateTo(int new_state)
{
if(crazyradio_status == CONNECTED)
{
- ROS_INFO("Change state to: %d", new_state);
+ ROS_INFO("[PPS CLIENT] Change state to: %d", new_state);
flying_state = new_state;
}
else
{
- ROS_INFO("Disconnected and trying to change state. State goes to MOTORS OFF");
+ ROS_INFO("[PPS CLIENT] Disconnected and trying to change state. State goes to MOTORS OFF");
flying_state = STATE_MOTORS_OFF;
}
@@ -225,7 +225,7 @@ void viconCallback(const ViconData& viconData) {
if(changed_state_flag) // stuff that will be run only once when changing state
{
changed_state_flag = false;
- ROS_INFO("STATE_MOTORS_OFF");
+ ROS_INFO("[PPS CLIENT] STATE_MOTORS_OFF");
}
break;
case STATE_TAKE_OFF: //we need to move up from where we are now.
@@ -234,7 +234,7 @@ void viconCallback(const ViconData& viconData) {
changed_state_flag = false;
takeOffCF(local);
finished_take_off = false;
- ROS_INFO("STATE_TAKE_OFF");
+ ROS_INFO("[PPS CLIENT] STATE_TAKE_OFF");
timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_take_off), takeOffTimerCallback, true);
}
if(finished_take_off)
@@ -250,7 +250,7 @@ void viconCallback(const ViconData& viconData) {
changed_state_flag = false;
// need to change setpoint to the controller one
goToControllerSetpoint();
- ROS_INFO("STATE_FLYING");
+ ROS_INFO("[PPS CLIENT] STATE_FLYING");
}
break;
case STATE_LAND:
@@ -259,7 +259,7 @@ void viconCallback(const ViconData& viconData) {
changed_state_flag = false;
landCF(local);
finished_land = false;
- ROS_INFO("STATE_LAND");
+ ROS_INFO("[PPS CLIENT] STATE_LAND");
timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_landing), landTimerCallback, true);
}
if(finished_land)
@@ -284,15 +284,15 @@ void viconCallback(const ViconData& viconData) {
bool success = demoController.call(controllerCall);
if(!success)
{
- ROS_ERROR("Failed to call demo controller, switching to safe controller");
- ROS_ERROR_STREAM("demo controller status: valid: " << demoController.isValid() << ", exists: " << demoController.exists());
- ROS_ERROR_STREAM("demo controller name: " << demoController.getService());
+ ROS_ERROR("[PPS CLIENT] Failed to call demo controller, switching to safe controller");
+ ROS_ERROR_STREAM("[PPS CLIENT] demo controller status: valid: " << demoController.isValid() << ", exists: " << demoController.exists());
+ ROS_ERROR_STREAM("[PPS CLIENT] demo controller name: " << demoController.getService());
setInstantController(SAFE_CONTROLLER);
}
else if(!safetyCheck(global, controllerCall.response.controlOutput))
{
setInstantController(SAFE_CONTROLLER);
- ROS_INFO_STREAM("safety check failed, switching to safe controller");
+ ROS_INFO_STREAM("[PPS CLIENT] safety check failed, switching to safe controller");
}
}
else //SAFE_CONTROLLER and state is different from flying
@@ -328,7 +328,7 @@ void viconCallback(const ViconData& viconData) {
bool success = safeController.call(controllerCall);
if(!success) {
- ROS_ERROR_STREAM("Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
+ ROS_ERROR_STREAM("[PPS CLIENT] Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
}
}
@@ -360,7 +360,7 @@ void viconCallback(const ViconData& viconData) {
void loadCrazyflieContext() {
CMQuery contextCall;
contextCall.request.studentID = agentID;
- ROS_INFO_STREAM("AgentID:" << agentID);
+ ROS_INFO_STREAM("[PPS CLIENT] AgentID:" << agentID);
CrazyflieContext new_context;
@@ -368,7 +368,7 @@ void loadCrazyflieContext() {
if(centralManager.call(contextCall)) {
new_context = contextCall.response.crazyflieContext;
- ROS_INFO_STREAM("CrazyflieContext:\n" << new_context);
+ ROS_INFO_STREAM("[PPS CLIENT] CrazyflieContext:\n" << new_context);
if((context.crazyflieName != "") && (new_context.crazyflieName != context.crazyflieName)) //linked crazyflie name changed and it was not empty before
{
@@ -380,7 +380,7 @@ void loadCrazyflieContext() {
// msg.data = CMD_CRAZYFLY_MOTORS_OFF;
// commandPublisher.publish(msg);
- ROS_INFO("CF is now different for this student. Disconnect and turn it off");
+ ROS_INFO("[PPS CLIENT] CF is now different for this student. Disconnect and turn it off");
std_msgs::Int32 msg;
msg.data = CMD_DISCONNECT;
@@ -394,7 +394,7 @@ void loadCrazyflieContext() {
}
else
{
- ROS_ERROR("Failed to load context. Waiting for next Save in DB by teacher");
+ ROS_ERROR("[PPS CLIENT] Failed to load context. Waiting for next Save in DB by teacher");
}
}
@@ -405,12 +405,12 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
switch(cmd) {
case CMD_USE_SAFE_CONTROLLER:
- ROS_INFO("USE_SAFE_CONTROLLER Command received");
+ ROS_INFO("[PPS CLIENT] USE_SAFE_CONTROLLER Command received");
setControllerUsed(SAFE_CONTROLLER);
break;
case CMD_USE_DEMO_CONTROLLER:
- ROS_INFO("USE_DEMO_CONTROLLER Command received");
+ ROS_INFO("[PPS CLIENT] USE_DEMO_CONTROLLER Command received");
setControllerUsed(DEMO_CONTROLLER);
break;
@@ -432,7 +432,7 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
break;
default:
- ROS_ERROR_STREAM("unexpected command number: " << cmd);
+ ROS_ERROR_STREAM("[PPS CLIENT] unexpected command number: " << cmd);
break;
}
}
@@ -502,10 +502,10 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
{
// > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
- case FETCH_YAML_SAFE_CONTROLLER_AGENT:
+ case FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT:
{
// Let the user know that this message was received
- ROS_INFO("The PPSClient received the message that YAML parameters were (re-)loaded for the Safe Controller. > Now fetching the parameter values from this machine.");
+ ROS_INFO("[PPS CLIENT] Received the message that YAML parameters were (re-)loaded for the Safe Controller. > Now fetching the parameter values from this machine.");
// 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
@@ -514,11 +514,11 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
}
// > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- case FETCH_YAML_SAFE_CONTROLLER_COORDINATOR:
+ case FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR:
{
// Let the user know that this message was received
// > and also from where the paramters are being fetched
- ROS_INFO("The PPSClient received the message that YAML parameters were (re-)loaded for the Safe Controller. > Now fetching the parameter values from the coordinator.");
+ ROS_INFO("[PPS CLIENT] Received the message that YAML parameters were (re-)loaded for the Safe Controller. > 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
@@ -546,27 +546,27 @@ void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle)
if(!nodeHandle_for_safeController.getParam("takeOffDistance", take_off_distance))
{
- ROS_ERROR("Failed to get takeOffDistance");
+ ROS_ERROR("[PPS CLIENT] Failed to get takeOffDistance");
}
if(!nodeHandle_for_safeController.getParam("landingDistance", landing_distance))
{
- ROS_ERROR("Failed to get landing_distance");
+ ROS_ERROR("[PPS CLIENT] Failed to get landing_distance");
}
if(!nodeHandle_for_safeController.getParam("durationTakeOff", duration_take_off))
{
- ROS_ERROR("Failed to get duration_take_off");
+ ROS_ERROR("[PPS CLIENT] Failed to get duration_take_off");
}
if(!nodeHandle_for_safeController.getParam("durationLanding", duration_landing))
{
- ROS_ERROR("Failed to get duration_landing");
+ ROS_ERROR("[PPS CLIENT] Failed to get duration_landing");
}
if(!nodeHandle_for_safeController.getParam("distanceThreshold", distance_threshold))
{
- ROS_ERROR("Failed to get distance_threshold");
+ ROS_ERROR("[PPS CLIENT] Failed to get distance_threshold");
}
}
@@ -575,22 +575,22 @@ void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle)
void fetchClientConfigParameters(ros::NodeHandle& nodeHandle)
{
if(!nodeHandle.getParam("agentID", agentID)) {
- ROS_ERROR("Failed to get agentID");
+ ROS_ERROR("[PPS CLIENT] Failed to get agentID");
}
if(!nodeHandle.getParam("strictSafety", strictSafety)) {
- ROS_ERROR("Failed to get strictSafety param");
+ ROS_ERROR("[PPS CLIENT] Failed to get strictSafety param");
return;
}
if(!nodeHandle.getParam("angleMargin", angleMargin)) {
- ROS_ERROR("Failed to get angleMargin param");
+ ROS_ERROR("[PPS CLIENT] Failed to get angleMargin param");
return;
}
if(!nodeHandle.getParam("battery_threshold_while_flying", m_battery_threshold_while_flying)) {
- ROS_ERROR("Failed to get battery_threshold_while_flying param");
+ ROS_ERROR("[PPS CLIENT] Failed to get battery_threshold_while_flying param");
return;
}
if(!nodeHandle.getParam("battery_threshold_while_motors_off", m_battery_threshold_while_motors_off)) {
- ROS_ERROR("Failed to get battery_threshold_while_motors_off param");
+ ROS_ERROR("[PPS CLIENT] Failed to get battery_threshold_while_motors_off param");
return;
}
}
@@ -635,16 +635,16 @@ void setBatteryStateTo(int new_battery_state)
{
case BATTERY_STATE_NORMAL:
m_battery_state = BATTERY_STATE_NORMAL;
- ROS_INFO("changed battery state to normal");
+ ROS_INFO("[PPS CLIENT] changed battery state to normal");
break;
case BATTERY_STATE_LOW:
m_battery_state = BATTERY_STATE_LOW;
- ROS_INFO("changed battery state to low");
+ ROS_INFO("[PPS CLIENT] changed battery state to low");
if(flying_state != STATE_MOTORS_OFF)
changeFlyingStateTo(STATE_LAND);
break;
default:
- ROS_INFO("Unknown battery state command, set to normal");
+ ROS_INFO("[PPS CLIENT] Unknown battery state command, set to normal");
m_battery_state = BATTERY_STATE_NORMAL;
break;
}
@@ -692,7 +692,7 @@ void CFBatteryCallback(const std_msgs::Float32& msg)
{
if(getBatteryState() != BATTERY_STATE_LOW)
setBatteryStateTo(BATTERY_STATE_LOW);
- ROS_INFO("low level battery triggered");
+ ROS_INFO("[PPS CLIENT] low level battery triggered");
}
else //maybe add hysteresis somewhere here?
{
@@ -720,13 +720,13 @@ void loadSafeController() {
std::string safeControllerName;
if(!nodeHandle.getParam("safeController", safeControllerName)) {
- ROS_ERROR("Failed to get safe controller name");
+ ROS_ERROR("[PPS CLIENT] Failed to get safe controller name");
return;
}
ros::service::waitForService(safeControllerName);
safeController = ros::service::createClient<Controller>(safeControllerName, true);
- ROS_INFO_STREAM("loaded safe controller: " << safeController.getService());
+ ROS_INFO_STREAM("[PPS CLIENT] loaded safe controller: " << safeController.getService());
}
void loadDemoController()
@@ -736,12 +736,12 @@ void loadDemoController()
std::string demoControllerName;
if(!nodeHandle.getParam("demoController", demoControllerName))
{
- ROS_ERROR("Failed to get demo controller name");
+ ROS_ERROR("[PPS CLIENT] Failed to get demo controller name");
return;
}
demoController = ros::service::createClient<Controller>(demoControllerName, true);
- ROS_INFO_STREAM("Loaded demo controller " << demoController.getService());
+ ROS_INFO_STREAM("[PPS CLIENT] Loaded demo controller " << demoController.getService());
}
@@ -874,7 +874,7 @@ int main(int argc, char* argv[])
if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
{
- ROS_ERROR_STREAM("The PPSClient could not find parameter 'defaultSetpoint', as called from main(...)");
+ ROS_ERROR_STREAM("[PPS CLIENT] Could not find parameter 'defaultSetpoint', as called from main(...)");
}
// Copy the default setpoint into the class variable
@@ -889,7 +889,7 @@ int main(int argc, char* argv[])
//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback);
- ROS_INFO_STREAM("successfully subscribed to ViconData");
+ ROS_INFO_STREAM("[PPS CLIENT] successfully subscribed to ViconData");
//ros::Publishers to advertise the control output
controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
diff --git a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
index 27cd8339..bf3aa7d9 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
@@ -81,7 +81,7 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// parameters should be loaded
int controller_to_load_yaml = msg.data;
- ROS_INFO_STREAM("The Parameter Service node received the message to load YAML parameters from file into cache");
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Received the message to load YAML parameters from file into cache");
// Instantiate a local varaible to confirm that something can actually be loaded
@@ -142,9 +142,9 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
else
{
// Let the user know that the command was not recognised
- ROS_INFO_STREAM("> Nothing to load for this parameter service with.");
- ROS_INFO_STREAM("> The message received has 'controller_to_load_yaml' = " << controller_to_load_yaml);
- ROS_INFO_STREAM("> And the type of this Parameter Service is 'my_type' = " << my_type);
+ ROS_INFO_STREAM("[PARAMETER SERVICE] > Nothing to load for this parameter service with.");
+ ROS_INFO_STREAM("[PARAMETER SERVICE] > The message received has 'controller_to_load_yaml' = " << controller_to_load_yaml);
+ ROS_INFO_STREAM("[PARAMETER SERVICE] > And the type of this Parameter Service is 'my_type' = " << my_type);
// Set the boolean that prevents the fetch message from being sent
isValidToAttemptLoad = false;
}
@@ -155,7 +155,7 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
if (isValidToAttemptLoad)
{
// Let the user know what is about to happen
- ROS_INFO_STREAM("> The following path will be used for locating the .yaml file: " << d_fall_pps_path << " The comand line string sent to the 'system' is: " << cmd );
+ ROS_INFO_STREAM("[PARAMETER SERVICE] > The following path will be used for locating the .yaml file: " << d_fall_pps_path << " The comand line string sent to the 'system' is: " << cmd );
// Re-load the parameters by pass the command line string via a "system" call
// > i.e., this replicates pasting this string in a new terminal window and pressing enter
@@ -228,7 +228,7 @@ int main(int argc, char* argv[])
// Get the namespace of this "ParameterService" node
std::string m_namespace = ros::this_node::getNamespace();
- ROS_INFO_STREAM("For ParameterService, ros::this_node::getNamespace() = " << m_namespace);
+ ROS_INFO_STREAM("[PARAMETER SERVICE] ros::this_node::getNamespace() = " << m_namespace);
@@ -237,7 +237,7 @@ int main(int argc, char* argv[])
if(!nodeHandle.getParam("type", type_string))
{
// Throw an error if the agent ID parameter could not be obtained
- ROS_ERROR("Failed to get type from ParameterService");
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get type from ParameterService");
}
// Set the "my_type" instance variable based on this string loaded
@@ -253,7 +253,7 @@ int main(int argc, char* argv[])
{
// Set "my_type" to the value indicating that it is invlid
my_type = TYPE_INVALID;
- ROS_ERROR("The 'type' parameter retrieved was not recognised.");
+ ROS_ERROR("[PARAMETER SERVICE] The 'type' parameter retrieved was not recognised.");
}
@@ -261,7 +261,7 @@ int main(int argc, char* argv[])
if(!nodeHandle.getParam("agentID", my_agentID))
{
// Throw an error if the agent ID parameter could not be obtained
- ROS_ERROR("Failed to get agentID from ParameterService");
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get agentID from ParameterService");
}
@@ -278,7 +278,7 @@ int main(int argc, char* argv[])
//m_base_namespace = ros::this_node::getNamespace();
//m_base_namespace = "/agent" + my_agentID + '/' + "ParameterService";
m_base_namespace = m_namespace + '/' + "ParameterService";
- ROS_INFO_STREAM("This Paramter Sercice will load .yaml file parameters into the 'base' namespace: " << m_base_namespace);
+ ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
break;
}
@@ -289,13 +289,13 @@ int main(int argc, char* argv[])
//m_base_namespace = ros::this_node::getNamespace();
//m_base_namespace = "/ParameterService";
m_base_namespace = m_namespace + '/' + "ParameterService";
- ROS_INFO_STREAM("This Paramter Sercice will load .yaml file parameters into the 'base' namespace: " << m_base_namespace);
+ ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
break;
}
default:
{
- ROS_ERROR("The 'my_type' type parameter was not recognised.");
+ ROS_ERROR("[PARAMETER SERVICE] The 'my_type' type parameter was not recognised.");
break;
}
}
@@ -333,7 +333,7 @@ int main(int argc, char* argv[])
requestLoadControllerYamlSubscriber_agent_to_coordinator = nh_coordinator_for_this_agent.subscribe("/my_GUI/requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
// Inform the user what was subscribed to:
- ROS_INFO_STREAM("This Parameter Service has subscribed to 'requestLoadControllerYaml' messages from both the 'my_GUI' and the 'PPSClient'");
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Subscribed to 'requestLoadControllerYaml' messages from both the 'my_GUI' and the 'PPSClient'");
break;
}
@@ -348,19 +348,19 @@ int main(int argc, char* argv[])
requestLoadControllerYamlSubscriber_coordinator_to_self = nh_coordinator_for_this_coordinator.subscribe("/my_GUI/requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
// Inform the user what was subscribed to:
- ROS_INFO_STREAM("This Parameter Service has subscribed to 'requestLoadControllerYaml' messages from 'my_GUI'");
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Subscribed to 'requestLoadControllerYaml' messages from 'my_GUI'");
break;
}
default:
{
- ROS_ERROR("The 'my_type' type parameter was not recognised.");
+ ROS_ERROR("[PARAMETER SERVICE] The 'my_type' type parameter was not recognised.");
break;
}
}
- ROS_INFO("CentralManagerService ready");
+ ROS_INFO("[PARAMETER SERVICE] Service ready :-)");
ros::spin();
return 0;
diff --git a/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
index 5e78c75b..1ac866ed 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
@@ -32,155 +32,11 @@
-// ----------------------------------------------------------------------------------
-// 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>
-
-
-
-
-// ----------------------------------------------------------------------------------
-// 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
-
-// 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;
-
-
-
-
-// ----------------------------------------------------------------------------------
-// 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]);
+// INCLUDE THE HEADER
+#include "nodes/SafeControllerService.h"
-// > 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);
@@ -383,10 +239,10 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
{
// > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
- case FETCH_YAML_SAFE_CONTROLLER_AGENT:
+ case FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT:
{
// Let the user know that this message was received
- ROS_INFO("The SafeControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this machine.");
+ ROS_INFO("[SAFE CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this machine.");
// 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
@@ -395,10 +251,10 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
}
// > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- case FETCH_YAML_SAFE_CONTROLLER_COORDINATOR:
+ case FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR:
{
// Let the user know that this message was received
- ROS_INFO("The SafeControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
+ ROS_INFO("[SAFE 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
@@ -448,7 +304,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_safeController, "defaultSetpoint", defaultSetpoint, 4);
// DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("DEBUGGING: the fetched SafeController/mass = " << cf_mass);
+ ROS_INFO_STREAM("[SAFE CONTROLLER] DEBUGGING: the fetched SafeController/mass = " << cf_mass);
// Call the function that computes details an values that are needed from these
// parameters loaded above
@@ -626,9 +482,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("The namespace string 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("[SAFE CONTROLLER] The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("[SAFE CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[SAFE CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
// FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
@@ -647,7 +503,7 @@ int main(int argc, char* argv[]) {
ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
- ROS_INFO("SafeControllerService ready");
+ ROS_INFO("[SAFE CONTROLLER] Service ready :-)");
// std::string package_path;
// package_path = ros::package::getPath("d_fall_pps") + "/";
diff --git a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
index 1a26ee43..e31a3b81 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
@@ -553,10 +553,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_STUDENT_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("[STUDENT 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
@@ -565,10 +565,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_STUDENT_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("[STUDENT 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
@@ -595,7 +595,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, "StudentController");
// > The mass of the crazyflie
cf_mass = getParameterFloat(nodeHandle_for_customController , "mass");
@@ -613,7 +613,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("[STUDENT CONTROLLER] DEBUGGING: the fetched CustomController/mass = " << cf_mass);
// Call the function that computes details an values that are needed from these
// parameters loaded above
@@ -632,7 +632,7 @@ void processFetchedParameters()
gravity_force = cf_mass * 9.81/(1000*4);
// DEBUGGING: Print out one of the computed quantities
- ROS_INFO_STREAM("DEBUGGING: thus the graity force = " << gravity_force);
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: thus the graity force = " << gravity_force);
}
@@ -722,12 +722,16 @@ bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
int main(int argc, char* argv[]) {
// Starting the ROS-node
- ros::init(argc, argv, "CustomControllerService");
+ ros::init(argc, argv, "StudentControllerService");
// Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
// the "~" indcates that "self" is the node handle assigned to this variable.
ros::NodeHandle nodeHandle("~");
+ // Get the namespace of this "StudentControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
// Get the agent ID as the "ROS_NAMESPACE" this computer.
// NOTES:
// > If you look at the "Student.launch" file in the "launch" folder, you will see
@@ -736,15 +740,13 @@ int main(int argc, char* argv[]) {
// This line of code adds a parameter named "studentID" to the "PPSClient"
// > Thus, to get access to this "studentID" paremeter, we first need to get a handle
// to the "PPSClient" node within which this controller service is nested.
- // Get the namespace of this "CustomControllerService" node
- std::string m_namespace = ros::this_node::getNamespace();
// Get the handle to the "PPSClient" node
ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
// Get the value of the "studentID" parameter into the instance variable "my_agentID"
- if(!PPSClient_nodeHandle.getParam("studentID", my_agentID))
+ if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
{
// Throw an error if the student ID parameter could not be obtained
- ROS_ERROR("Failed to get studentID from FollowN_1Service");
+ ROS_ERROR("[STUDENT CONTROLLER] Failed to get agentID from PPSClient");
}
@@ -795,9 +797,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("The namespace string 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("[STUDENT CONTROLLER] The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
// FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
@@ -837,7 +839,7 @@ int main(int argc, char* argv[]) {
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
// Print out some information to the user.
- ROS_INFO("CustomControllerService ready");
+ ROS_INFO("[STUDENT CONTROLLER] Service ready :-)");
// Enter an endless while loop to keep the node alive.
ros::spin();
--
GitLab