Commit d7d3b2d2 authored by roangel's avatar roangel
Browse files

changed way of changing controllers. need to test

parent 612dfe49
1,PPS_CF02,0/8/2M/E7E7E7E702,0,-1.38022,-0.405516,-0.2,-0.481301,0.485645,2
2,PPS_CF04,0/24/2M,1,-0.412714,-0.376959,-0.2,0.423148,0.384735,2
5,cfTwo,0/69/2M,0,-0.251699,-1.96387,-0.2,0.0792386,-1.54593,2
......@@ -123,7 +123,7 @@ CrazyflieContext context;
int instant_controller; //variable for the instant controller, e.g., we use safe controller for taking off and landing even if custom controller is enabled. This variable WILL change automatically
// controller used:
int controller_used = SAFE_CONTROLLER;
int controller_used;
ros::Publisher controllerUsedPublisher;
......@@ -141,36 +141,6 @@ ros::Timer timer_takeoff;
ros::Timer timer_land;
void setControllerUsed(int controller)
{
controller_used = controller;
}
int getControllerUsed()
{
return controller_used;
}
void useSafeController()
{
setControllerUsed(SAFE_CONTROLLER);
}
void sendMessageUsingController(int controller)
{
// send a message in topic for the studentGUI to read it
std_msgs::Int32 controller_used_msg;
controller_used_msg.data = controller;
controllerUsedPublisher.publish(controller_used_msg);
}
void useCustomController()
{
setControllerUsed(CUSTOM_CONTROLLER);
}
void loadSafeController() {
ros::NodeHandle nodeHandle("~");
......@@ -201,6 +171,48 @@ void loadCustomController()
}
void sendMessageUsingController(int controller)
{
// send a message in topic for the studentGUI to read it
std_msgs::Int32 controller_used_msg;
controller_used_msg.data = controller;
controllerUsedPublisher.publish(controller_used_msg);
}
void setInstantController(int controller) //for right now, temporal use
{
instant_controller = controller;
sendMessageUsingController(controller);
switch(controller)
{
case SAFE_CONTROLLER:
loadSafeController();
break;
case CUSTOM_CONTROLLER:
loadCustomController();
break;
default:
break;
}
}
int getInstantController()
{
return instant_controller;
}
void setControllerUsed(int controller) //for permanent configs
{
controller_used = controller;
}
int getControllerUsed()
{
return controller_used;
}
//checks if crazyflie is within allowed area and if custom controller returns no data
bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
//position check
......@@ -284,8 +296,8 @@ void takeOffCF(CrazyflieData& current_local_coordinates) //local because the set
ROS_INFO("X: %f, Y: %f, Z: %f", setpoint_msg.x, setpoint_msg.y, setpoint_msg.z);
// now, use safe controller to go to that setpoint
useSafeController();
loadSafeController();
setInstantController(SAFE_CONTROLLER);
// when do we finish? after some time with delay?
// update variable that keeps track of current setpoint
......@@ -303,8 +315,8 @@ void landCF(CrazyflieData& current_local_coordinates)
safeControllerServiceSetpointPublisher.publish(setpoint_msg);
// now, use safe controller to go to that setpoint
useSafeController();
loadSafeController();
setInstantController(SAFE_CONTROLLER);
setCurrentSafeSetpoint(setpoint_msg);
}
......@@ -380,6 +392,7 @@ void viconCallback(const ViconData& viconData) {
if(finished_take_off)
{
finished_take_off = false;
setInstantController(getControllerUsed());
changeFlyingStateTo(STATE_FLYING);
}
break;
......@@ -404,6 +417,7 @@ void viconCallback(const ViconData& viconData) {
if(finished_land)
{
finished_land = false;
setInstantController(getControllerUsed());
changeFlyingStateTo(STATE_MOTORS_OFF);
}
break;
......@@ -411,14 +425,12 @@ void viconCallback(const ViconData& viconData) {
break;
}
sendMessageUsingController(getControllerUsed());
// control part that should always be running, calls to controller, computation of control output
if(flying_state != STATE_MOTORS_OFF)
{
if(!global.occluded) //if it is not occluded, then proceed to compute the controller output.
{
if(getControllerUsed() == CUSTOM_CONTROLLER && flying_state == STATE_FLYING) // only enter in custom controller if we are not using safe controller AND the flying state is FLYING
if(getInstantController() == CUSTOM_CONTROLLER && flying_state == STATE_FLYING) // only enter in custom controller if we are not using safe controller AND the flying state is FLYING
{
bool success = customController.call(controllerCall);
if(!success)
......@@ -426,11 +438,11 @@ void viconCallback(const ViconData& viconData) {
ROS_ERROR("Failed to call custom controller, switching to safe controller");
ROS_ERROR_STREAM("custom controller status: valid: " << customController.isValid() << ", exists: " << customController.exists());
ROS_ERROR_STREAM("custom controller name: " << customController.getService());
useSafeController();
setInstantController(SAFE_CONTROLLER);
}
else if(!safetyCheck(global, controllerCall.response.controlOutput))
{
useSafeController();
setInstantController(SAFE_CONTROLLER);
ROS_INFO_STREAM("safety check failed, switching to safe controller");
}
}
......@@ -580,14 +592,12 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
switch(cmd) {
case CMD_USE_SAFE_CONTROLLER:
ROS_INFO("USE_SAFE_CONTROLLER Command received");
loadSafeController();
useSafeController();
setControllerUsed(SAFE_CONTROLLER);
break;
case CMD_USE_CUSTOM_CONTROLLER:
ROS_INFO("USE_CUSTOM_CONTROLLER Command received");
loadCustomController();
useCustomController();
setControllerUsed(CUSTOM_CONTROLLER);
break;
case CMD_CRAZYFLY_TAKE_OFF:
......@@ -727,9 +737,8 @@ int main(int argc, char* argv[])
//start with safe controller
flying_state = STATE_MOTORS_OFF;
useSafeController();
loadSafeController();
sendMessageUsingController(getControllerUsed());
setControllerUsed(SAFE_CONTROLLER);
setInstantController(SAFE_CONTROLLER); //initialize this also, so we notify GUI
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
......
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