Commit 3a9e1f3e authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Added switching between four controller to PPSClient

parent a4a8bda8
...@@ -83,8 +83,10 @@ ...@@ -83,8 +83,10 @@
#define CF_COMMAND_TYPE_ANGLE 8 #define CF_COMMAND_TYPE_ANGLE 8
// Types of controllers being used: // Types of controllers being used:
#define SAFE_CONTROLLER 0 #define SAFE_CONTROLLER 0
#define DEMO_CONTROLLER 1 #define DEMO_CONTROLLER 1
#define STUDENT_CONTROLLER 3
#define MPC_CONTROLLER 4
// The constants that "command" changes in the // The constants that "command" changes in the
// operation state of this agent // operation state of this agent
...@@ -139,10 +141,15 @@ using namespace d_fall_pps; ...@@ -139,10 +141,15 @@ using namespace d_fall_pps;
// "agentID", gives namespace and identifier in CentralManagerService // "agentID", gives namespace and identifier in CentralManagerService
int agentID; int agentID;
// The safe controller specified in the ClientConfig.yaml, is considered stable // The safe controller specified in the ClientConfig.yaml
ros::ServiceClient safeController; 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; 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 //values for safteyCheck
bool strictSafety; bool strictSafety;
...@@ -281,6 +288,9 @@ void CFBatteryCallback(const std_msgs::Float32& msg); ...@@ -281,6 +288,9 @@ void CFBatteryCallback(const std_msgs::Float32& msg);
void loadSafeController(); void loadSafeController();
void loadDemoController(); void loadDemoController();
void loadStudentController();
void loadMpcController();
void sendMessageUsingController(int controller); void sendMessageUsingController(int controller);
void setInstantController(int controller); void setInstantController(int controller);
int getInstantController(); int getInstantController();
......
safeController: "SafeControllerService/RateController" safeController: "SafeControllerService/RateController"
demoController: "DemoControllerService/DemoController" demoController: "DemoControllerService/DemoController"
studentController: "StudentControllerService/StudentController"
mpcController: "MpcControllerService/MpcController"
strictSafety: false strictSafety: false
angleMargin: 0.6 angleMargin: 0.6
battery_threshold_while_flying: 2.8 # in V battery_threshold_while_flying: 2.8 # in V
battery_threshold_while_motors_off: 3.30 # in V battery_threshold_while_motors_off: 3.30 # in V
battery_polling_period: 100 # in ms battery_polling_period: 100 # in ms
......
...@@ -1013,7 +1013,7 @@ int main(int argc, char* argv[]) { ...@@ -1013,7 +1013,7 @@ int main(int argc, char* argv[]) {
// that should be sent via the Crazyradio and requested from the Crazyflie, i.e., // 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 // this is where the "outer loop" controller function starts. When a request is made
// of this service the "calculateControlOutput" function is called. // 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 // 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: // to the name space of this node, i.e., "d_fall_pps" as specified by the line:
......
...@@ -278,15 +278,36 @@ void viconCallback(const ViconData& viconData) { ...@@ -278,15 +278,36 @@ void viconCallback(const ViconData& viconData) {
{ {
if(!global.occluded) //if it is not occluded, then proceed to compute the controller output. 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 // Only call an "non-safe" controller if:
if(getInstantController() == DEMO_CONTROLLER && flying_state == STATE_FLYING) // 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) if(!success)
{ {
ROS_ERROR("[PPS CLIENT] Failed to call demo controller, switching to safe controller"); ROS_ERROR("[PPS CLIENT] Failed to call a 'non-safe' controller, switching to safe controller");
ROS_ERROR_STREAM("[PPS CLIENT] demo controller status: valid: " << demoController.isValid() << ", exists: " << demoController.exists()); //ROS_ERROR_STREAM("[PPS CLIENT] 'non-safe' controller status: valid: " << demoController.isValid() << ", exists: " << demoController.exists());
ROS_ERROR_STREAM("[PPS CLIENT] demo controller name: " << demoController.getService()); //ROS_ERROR_STREAM("[PPS CLIENT] 'non-safe' controller name: " << demoController.getService());
setInstantController(SAFE_CONTROLLER); setInstantController(SAFE_CONTROLLER);
} }
else if(!safetyCheck(global, controllerCall.response.controlOutput)) else if(!safetyCheck(global, controllerCall.response.controlOutput))
...@@ -294,8 +315,11 @@ void viconCallback(const ViconData& viconData) { ...@@ -294,8 +315,11 @@ void viconCallback(const ViconData& viconData) {
setInstantController(SAFE_CONTROLLER); setInstantController(SAFE_CONTROLLER);
ROS_INFO_STREAM("[PPS CLIENT] 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 // SAFE_CONTROLLER and state is different from flying
else
{ {
calculateDistanceToCurrentSafeSetpoint(local); // update distance, it also updates the unit vector calculateDistanceToCurrentSafeSetpoint(local); // update distance, it also updates the unit vector
// ROS_INFO_STREAM("distance: " << distance); // ROS_INFO_STREAM("distance: " << distance);
...@@ -414,6 +438,16 @@ void commandCallback(const std_msgs::Int32& commandMsg) { ...@@ -414,6 +438,16 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
setControllerUsed(DEMO_CONTROLLER); setControllerUsed(DEMO_CONTROLLER);
break; 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: case CMD_CRAZYFLY_TAKE_OFF:
if(flying_state == STATE_MOTORS_OFF) if(flying_state == STATE_MOTORS_OFF)
{ {
...@@ -744,6 +778,35 @@ void loadDemoController() ...@@ -744,6 +778,35 @@ void loadDemoController()
ROS_INFO_STREAM("[PPS CLIENT] Loaded demo controller " << demoController.getService()); 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) void sendMessageUsingController(int controller)
{ {
...@@ -765,6 +828,12 @@ void setInstantController(int controller) //for right now, temporal use ...@@ -765,6 +828,12 @@ void setInstantController(int controller) //for right now, temporal use
case DEMO_CONTROLLER: case DEMO_CONTROLLER:
loadDemoController(); loadDemoController();
break; break;
case STUDENT_CONTROLLER:
loadStudentController();
break;
case MPC_CONTROLLER:
loadMpcController();
break;
default: default:
break; break;
} }
......
...@@ -830,7 +830,7 @@ int main(int argc, char* argv[]) { ...@@ -830,7 +830,7 @@ int main(int argc, char* argv[]) {
// that should be sent via the Crazyradio and requested from the Crazyflie, i.e., // 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 // this is where the "outer loop" controller function starts. When a request is made
// of this service the "calculateControlOutput" function is called. // 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 // 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: // to the name space of this node, i.e., "d_fall_pps" as specified by the line:
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment