From 3a9e1f3e854409b4e251edb39283ebfa223f16d3 Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Wed, 25 Apr 2018 09:49:59 +0200
Subject: [PATCH] Added switching between four controller to PPSClient
---
.../src/d_fall_pps/include/nodes/PPSClient.h | 18 +++-
pps_ws/src/d_fall_pps/param/ClientConfig.yaml | 8 +-
.../src/nodes/MpcControllerService.cpp | 2 +-
pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp | 83 +++++++++++++++++--
.../src/nodes/StudentControllerService.cpp | 2 +-
5 files changed, 98 insertions(+), 15 deletions(-)
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 46a65d04..d87085d5 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
@@ -83,8 +83,10 @@
#define CF_COMMAND_TYPE_ANGLE 8
// Types of controllers being used:
-#define SAFE_CONTROLLER 0
-#define DEMO_CONTROLLER 1
+#define SAFE_CONTROLLER 0
+#define DEMO_CONTROLLER 1
+#define STUDENT_CONTROLLER 3
+#define MPC_CONTROLLER 4
// The constants that "command" changes in the
// operation state of this agent
@@ -139,10 +141,15 @@ using namespace d_fall_pps;
// "agentID", gives namespace and identifier in CentralManagerService
int agentID;
-// The safe controller specified in the ClientConfig.yaml, is considered stable
+// The safe controller specified in the ClientConfig.yaml
ros::ServiceClient safeController;
-// The Demo controller specified in the ClientConfig.yaml, is considered potentially unstable
+// The Demo controller specified in the ClientConfig.yaml
ros::ServiceClient demoController;
+// The Demo controller specified in the ClientConfig.yaml
+ros::ServiceClient studentController;
+// The Demo controller specified in the ClientConfig.yaml
+ros::ServiceClient mpcController;
+
//values for safteyCheck
bool strictSafety;
@@ -281,6 +288,9 @@ void CFBatteryCallback(const std_msgs::Float32& msg);
void loadSafeController();
void loadDemoController();
+void loadStudentController();
+void loadMpcController();
+
void sendMessageUsingController(int controller);
void setInstantController(int controller);
int getInstantController();
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index a32012ed..101820c3 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -1,7 +1,11 @@
-safeController: "SafeControllerService/RateController"
-demoController: "DemoControllerService/DemoController"
+safeController: "SafeControllerService/RateController"
+demoController: "DemoControllerService/DemoController"
+studentController: "StudentControllerService/StudentController"
+mpcController: "MpcControllerService/MpcController"
+
strictSafety: false
angleMargin: 0.6
+
battery_threshold_while_flying: 2.8 # in V
battery_threshold_while_motors_off: 3.30 # in V
battery_polling_period: 100 # in ms
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 f2aa6049..a4a2eb6a 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
@@ -1013,7 +1013,7 @@ int main(int argc, char* argv[]) {
// that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
// this is where the "outer loop" controller function starts. When a request is made
// of this service the "calculateControlOutput" function is called.
- ros::ServiceServer service = nodeHandle.advertiseService("CustomController", calculateControlOutput);
+ ros::ServiceServer service = nodeHandle.advertiseService("MpcController", calculateControlOutput);
// Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
// to the name space of this node, i.e., "d_fall_pps" as specified by the line:
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 ef718af6..abccda99 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
@@ -278,15 +278,36 @@ void viconCallback(const ViconData& viconData) {
{
if(!global.occluded) //if it is not occluded, then proceed to compute the controller output.
{
- // only enter in demo controller if we are not using safe controller AND the flying state is FLYING
- if(getInstantController() == DEMO_CONTROLLER && flying_state == STATE_FLYING)
+ // Only call an "non-safe" controller if:
+ // 1) we are not currently using safe controller, AND
+ // 2) the flying state is FLYING
+ if( (getInstantController() != SAFE_CONTROLLER) && (flying_state == STATE_FLYING) )
{
- bool success = demoController.call(controllerCall);
+ // Initialise a local boolean success variable
+ bool success = false;
+
+ switch(getInstantController())
+ {
+ case DEMO_CONTROLLER:
+ success = demoController.call(controllerCall);
+ break;
+ case STUDENT_CONTROLLER:
+ success = studentController.call(controllerCall);
+ break;
+ case MPC_CONTROLLER:
+ success = mpcController.call(controllerCall);
+ break;
+ default:
+ ROS_ERROR("[PPS CLIENT] the current controller was not recognised");
+ break;
+ }
+
+ // Ensure success and enforce safety
if(!success)
{
- 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());
+ ROS_ERROR("[PPS CLIENT] Failed to call a 'non-safe' controller, switching to safe controller");
+ //ROS_ERROR_STREAM("[PPS CLIENT] 'non-safe' controller status: valid: " << demoController.isValid() << ", exists: " << demoController.exists());
+ //ROS_ERROR_STREAM("[PPS CLIENT] 'non-safe' controller name: " << demoController.getService());
setInstantController(SAFE_CONTROLLER);
}
else if(!safetyCheck(global, controllerCall.response.controlOutput))
@@ -294,8 +315,11 @@ void viconCallback(const ViconData& viconData) {
setInstantController(SAFE_CONTROLLER);
ROS_INFO_STREAM("[PPS CLIENT] safety check failed, switching to safe controller");
}
+
+
}
- else //SAFE_CONTROLLER and state is different from flying
+ // SAFE_CONTROLLER and state is different from flying
+ else
{
calculateDistanceToCurrentSafeSetpoint(local); // update distance, it also updates the unit vector
// ROS_INFO_STREAM("distance: " << distance);
@@ -414,6 +438,16 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
setControllerUsed(DEMO_CONTROLLER);
break;
+ case CMD_USE_STUDENT_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_STUDENT_CONTROLLER Command received");
+ setControllerUsed(STUDENT_CONTROLLER);
+ break;
+
+ case CMD_USE_MPC_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_MPC_CONTROLLER Command received");
+ setControllerUsed(MPC_CONTROLLER);
+ break;
+
case CMD_CRAZYFLY_TAKE_OFF:
if(flying_state == STATE_MOTORS_OFF)
{
@@ -744,6 +778,35 @@ void loadDemoController()
ROS_INFO_STREAM("[PPS CLIENT] Loaded demo controller " << demoController.getService());
}
+void loadStudentController()
+{
+ ros::NodeHandle nodeHandle("~");
+
+ std::string studentControllerName;
+ if(!nodeHandle.getParam("studentController", studentControllerName))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get student controller name");
+ return;
+ }
+
+ studentController = ros::service::createClient<Controller>(studentControllerName, true);
+ ROS_INFO_STREAM("[PPS CLIENT] Loaded student controller " << studentController.getService());
+}
+
+void loadMpcController()
+{
+ ros::NodeHandle nodeHandle("~");
+
+ std::string mpcControllerName;
+ if(!nodeHandle.getParam("mpcController", mpcControllerName))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get mpc controller name");
+ return;
+ }
+
+ mpcController = ros::service::createClient<Controller>(mpcControllerName, true);
+ ROS_INFO_STREAM("[PPS CLIENT] Loaded mpc controller " << mpcController.getService());
+}
void sendMessageUsingController(int controller)
{
@@ -765,6 +828,12 @@ void setInstantController(int controller) //for right now, temporal use
case DEMO_CONTROLLER:
loadDemoController();
break;
+ case STUDENT_CONTROLLER:
+ loadStudentController();
+ break;
+ case MPC_CONTROLLER:
+ loadMpcController();
+ break;
default:
break;
}
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 e31a3b81..1bfb40a7 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
@@ -830,7 +830,7 @@ int main(int argc, char* argv[]) {
// that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
// this is where the "outer loop" controller function starts. When a request is made
// of this service the "calculateControlOutput" function is called.
- ros::ServiceServer service = nodeHandle.advertiseService("CustomController", calculateControlOutput);
+ ros::ServiceServer service = nodeHandle.advertiseService("StudentController", calculateControlOutput);
// Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
// to the name space of this node, i.e., "d_fall_pps" as specified by the line:
--
GitLab