Commit f248fd16 authored by phfriedl's avatar phfriedl
Browse files

added scripts to enable/disable crazyflie without stopping ROS

parent 48f5ab98
#!/bin/bash
if [ "$#" -ne 0 ]
then echo "usage: disable_crazyflie <no arguments>"
else rostopic pub -1 /PPSClient/Command std_msgs/Int32 4;
fi
#!/bin/bash
if [ "$#" -ne 0 ]
then echo "usage: enable_crazyflie <no arguments>"
else rostopic pub -1 /PPSClient/Command std_msgs/Int32 3;
fi
......@@ -29,6 +29,8 @@
#define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_CUSTOM_CONTROLLER 2
#define CMD_USE_CRAZYFLY_ENABLE 3
#define CMD_USE_CRAZYFLY_DISABLE 4
using namespace d_fall_pps;
......@@ -49,6 +51,8 @@ ros::Publisher controlCommandPublisher;
CrazyflieContext context;
//wheter to use safe of custom controller
bool usingSafeController;
//wheter crazyflie is enabled (ready to fly) or disabled (motors off)
bool crazyflieEnabled;
//checks if crazyflie is within allowed area and if custom controller returns no data
bool safetyCheck(ViconData data, ControlCommand controlCommand) {
......@@ -74,25 +78,31 @@ void viconCallback(const ViconData& data) {
Controller controllerCall;
controllerCall.request.crazyflieLocation = data;
if(!usingSafeController) {
bool success = customController.call(controllerCall);
if(!success) {
ROS_ERROR("Failed to call custom controller, switching to safe controller");
usingSafeController = true;
}
if(crazyflieEnabled){
if(!usingSafeController) {
bool success = customController.call(controllerCall);
if(!success) {
ROS_ERROR("Failed to call custom controller, switching to safe controller");
usingSafeController = true;
}
//usingSafeController = !safetyCheck(data, controllerCall.response.controlOutput);
usingSafeController = true; //debug
}
//usingSafeController = !safetyCheck(data, controllerCall.response.controlOutput);
usingSafeController = true; //debug
}
if(usingSafeController) {
bool success = safeController.call(controllerCall);
if(!success) {
ROS_ERROR("Failed to call safe controller");
if(usingSafeController) {
bool success = safeController.call(controllerCall);
if(!success) {
ROS_ERROR("Failed to call safe controller");
}
}
}
controlCommandPublisher.publish(controllerCall.response.controlOutput);
controlCommandPublisher.publish(controllerCall.response.controlOutput);
} else{ //crazyflie disabled
ControlCommand zeroOutput = ControlCommand(); //everything set to zero
zeroOutput.onboardControllerType = 2; //set to motor_mode
controlCommandPublisher.publish(zeroOutput);
}
}
}
......@@ -156,6 +166,14 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
usingSafeController = false;
break;
case CMD_USE_CRAZYFLY_ENABLE:
crazyflieEnabled = true;
break;
case CMD_USE_CRAZYFLY_DISABLE:
crazyflieEnabled = false;
break;
default:
ROS_ERROR_STREAM("unexpected command number: " << cmd);
break;
......@@ -179,6 +197,7 @@ int main(int argc, char* argv[]){
ros::Subscriber commandSubscriber = nodeHandle.subscribe("/PPSClient/Command", 1, commandCallback);
//start with safe controller
crazyflieEnabled = true;
usingSafeController = true;
loadSafeController();
......
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