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

Added switching between four controller to PPSClient

parent a4a8bda8
......@@ -85,6 +85,8 @@
// Types of controllers being used:
#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();
......
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
......
......@@ -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:
......
......@@ -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;
}
......
......@@ -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:
......
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