Commit f0fb2d88 authored by bucyril's avatar bucyril
Browse files

added ability to change safe controller setpoint through terminal

parent 77f48a8f
......@@ -91,6 +91,7 @@ add_message_files(
ControlCommand.msg
CrazyflieContext.msg
AreaBounds.msg
Setpoint.msg
)
## Generate services in the 'srv' folder
......
float64 x
float64 y
float64 z
float64 yaw
......@@ -15,5 +15,5 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw)
setpoint: [0.0, 0.0, 0.3, 1]
defaultSetpoint: [0.0, 0.0, 0.3, 1]
......@@ -128,8 +128,6 @@ void viconCallback(const ViconData& data){
else {
ROS_INFO_STREAM("ViconData from other crazyflie received: " << data.crazyflieName);
}
}
int main(int argc, char* argv[]){
......@@ -147,7 +145,7 @@ int main(int argc, char* argv[]){
ROS_ERROR("Failed to get CrazyFlieName");
}
ros::Subscriber ViconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback);
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback);
ROS_INFO_STREAM("successfully subscribed to ViconData");
//ros::Publishers to advertise the control output
......
......@@ -74,7 +74,7 @@ void loadParameters(ros::NodeHandle& nodeHandle) {
loadParameterFloatVector(nodeHandle, "filterGain", filterGain, 6);
loadParameterFloatVector(nodeHandle, "estimatorMatrix", estimatorMatrix, 2);
loadParameterFloatVector(nodeHandle, "setpoint", setpoint, 4);
loadParameterFloatVector(nodeHandle, "defaultSetpoint", setpoint, 4);
}
float computeMotorPolyBackward(float thrust) {
......@@ -259,6 +259,15 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
return true;
}
void setpointCallback(const Setpoint& newSetpoint) {
setpoint[0] = newSetpoint.x;
setpoint[1] = newSetpoint.y;
setpoint[2] = newSetpoint.z;
setpoint[3] = newSetpoint.yaw;
}
//ros::Publisher pub;
int main(int argc, char* argv[]) {
ros::init(argc, argv, "SafeControllerService");
......@@ -266,6 +275,12 @@ int main(int argc, char* argv[]) {
ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle);
ROS_INFO("<aaaaaaaa");
ros::Publisher setpointPublisher = nodeHandle.advertise<Setpoint>("Setpoint", 1);
ros::Subscriber setpointSubscriber = nodeHandle.subscribe("/SafeControllerService/Setpoint", 1, setpointCallback);
ROS_INFO("<aaaaaaabbbbbbbb");
ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
ROS_INFO("SafeControllerService ready");
ros::spin();
......
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