Skip to content
GitLab
Commit 2f3e2ce1 authored by muelmarc's avatar muelmarc
Browse files

safeRateCommandPkg added as Output from safeController. This gets then... 

safeRateCommandPkg added as Output from safeController. This gets then pubslished via callbackRateCommand.
parent 4c4a526b
Hide whitespace changes
Inline Side-by-side
pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
View file @ 2f3e2ce1
......@@ -94,7 +94,7 @@ class PPSRadioClient:
def motorCommandCallback(data):
"""Callback for motor controller actions"""
cf_client._send_to_commander(0, 0, 0, 0, data.cmd1, data.cmd2, data.cmd3, data.cmd4, CONTROLLER_MOTOR)
#cf_client._send_to_commander(0, 0, 0, 0, data.cmd1, data.cmd2, data.cmd3, data.cmd4, CONTROLLER_MOTOR)
rospy.loginfo("motor controller callback: %s, %s, %s, %s", data.cmd1, data.cmd2, data.cmd3, data.cmd4)
def angleCommandCallback(data):
......@@ -106,8 +106,10 @@ def angleCommandCallback(data):
def rateCommandCallback(data):
"""Callback for rate controller actions"""
#cmd1..4 must not be 0, as crazyflie onboard controller resets
#cf_client._send_to_commander(data.rollRate,data.pitchRate,data.yawRate,data.thrust, 1, 1, 1 , 1, CONTROLLER_RATE)
rospy.loginfo("rate controller callback: %s, %s, %s, %s", data.rollRate, data.pitchRate, data.yawRate, data.thrust)
#cf_client._send_to_commander(data.rollRate,data.pitchRate,data.yawRate,data.thrust, 1, 1, 1, 1, CONTROLLER_RATE)
cf_client._send_to_commander(0,0,0,0, 20000, 20000, 0, 0, 2)
#ACHTUNG: mode ist auf 2 (CONTROLLER_MOTOR), da die rateCommandCallback zum testen verwendet wurde!!!!!!!!!!!!!!!!!!!!!!!!
rospy.loginfo("rate controller callback : %s, %s, %s, %s", data.rollRate, data.pitchRate, data.yawRate, data.thrust)
if __name__ == '__main__':
rospy.init_node('CrazyRadio', anonymous=True)
......
pps_ws/src/d_fall_pps/src/PPSClient.cpp
View file @ 2f3e2ce1
......@@ -47,13 +47,16 @@ std::string team; //is this needed here? maybe for room asignment received from
std::string cflie;
//global sevices
ros::ServiceClient rateClient;
ros::ServiceClient safeController;
ros::ServiceClient centralClient;
ros::Publisher angleCommandPublisher;
ros::Publisher rateCommandPublisher;
ros::Publisher motorCommandPublisher;
//msg for safeController Output
d_fall_pps::RateCommand safeRateCommandPkg;
//uncommenting the next line causes FATAL Error at runtime: "You must call ros::init() before creating the first NodeHandle"
//ros::NodeHandle nodeHandle;
......@@ -72,6 +75,7 @@ AreaBoundaries Area;
//struct consistent with dusans controller
/*
struct ControllerOutput
{
//ControllerOutput():roll(0),pitch(0),yaw(0),thrust(0),motorCmd1(0),motorCmd2(0),motorCmd3(0),motorCmd4(0) {}
......@@ -86,7 +90,7 @@ struct ControllerOutput
uint8_t onboardControllerType;
};
ControllerOutput ControlCommandTest;
ControllerOutput ControlCommandTest;*/
//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<move this section to a separate file that is included
......@@ -94,44 +98,51 @@ ControllerOutput ControlCommandTest;
//not void: sould give back controlldata
void ppsClientToController(ViconData data, bool autocontrolOn){
if(data.crazyflieName == cflie){
//call safecontroller if autocontrol is true
//safeRateCommandPkg.motorCmd1 = 0;
//safeRateCommandPkg.motorCmd2 = 0;
//safeRateCommandPkg.motorCmd3 = 0;
//safeRateCommandPkg.motorCmd4 = 0;
if(autocontrolOn){
//call safecontroller if autocontrol is true
ControlCommandTest.motorCmd1 = 0;
ControlCommandTest.motorCmd2 = 0;
ControlCommandTest.motorCmd3 = 0;
ControlCommandTest.motorCmd4 = 0;
return;
//call safecontroller here
}
else {
//
ControlCommandTest.motorCmd1 = 1000;
ControlCommandTest.motorCmd2 = 1000;
ControlCommandTest.motorCmd3 = 1000;
ControlCommandTest.motorCmd4 = 1000;
//student controller is called here
//for the moment use safecontroller for TESTING
RateController srvRate;
Setpoint goalLocation;
goalLocation.x = -5; //testvalue
goalLocation.y = 250; //testvalue
goalLocation.z = 300; //testvalue
srvRate.request.crazyflieLocation = data;
srvRate.request.setpoint = goalLocation;
//TODO:
//return control commands
if(safeController.call(srvRate)){
ROS_INFO("Received control input");
ROS_INFO_STREAM(srvRate.response.controlOutput);
safeRateCommandPkg.rollRate = srvRate.response.controlOutput.rollRate;
safeRateCommandPkg.pitchRate = srvRate.response.controlOutput.pitchRate;
safeRateCommandPkg.yawRate = srvRate.response.controlOutput.yawRate;
safeRateCommandPkg.thrust = srvRate.response.controlOutput.thrust;
//onboardControllerType = ??????????????????????
}
else{
ROS_ERROR("Failed to call SafeControllerService");
//return 1; //return some useful stuff
}
}
//TODO:
//communicating with Controller
RateController srvRate;
Setpoint goalLocation;
goalLocation.x = 9; //testvalue
goalLocation.y = 8; //testvalue
goalLocation.z = 7; //testvalue
srvRate.request.crazyflieLocation = data;
srvRate.request.setpoint = goalLocation;
//TODO:
//return control commands
if(rateClient.call(srvRate)){
ROS_INFO("Service gave response");
ROS_INFO("Received control input");
ROS_INFO_STREAM(srvRate.response.controlOutput);
}
else{
ROS_ERROR("Failed to call SafeControllerService");
//return 1; //return some useful stuff
}
}
else {
ROS_INFO("ViconData from other crazyflie received");
......@@ -197,26 +208,28 @@ void callbackAngleCommand(const ros::TimerEvent&)
}
//callback method to publish d_fall_pps::RateCommand
void callbackRateCommand(const ros::TimerEvent&)
void callbackRateCommand(const ros::TimerEvent)
{
d_fall_pps::RateCommand rateCommandPkg;
rateCommandPkg.rollRate = 2;
rateCommandPkg.pitchRate = 2;
rateCommandPkg.yawRate = 2;
rateCommandPkg.thrust = 50;
//d_fall_pps::RateCommand rateCommandPkg;
//rateCommandPkg.rollRate = ...;
//rateCommandPkg.pitchRate = ...;
//rateCommandPkg.yawRate = ...;
//rateCommandPkg.thrust = ...,
rateCommandPublisher.publish(rateCommandPkg);
ROS_INFO_STREAM("RateCommandTimer pubslishes: " << rateCommandPkg.rollRate << ", " << rateCommandPkg.pitchRate << ", " << rateCommandPkg.yawRate);
//Achtung: gepublisht wird safeRateCommandPkg vom Type d_fall_pps::RateCommand >>>> brauchen wir eine separate callback fuer die commands des safetyController benoetigt?????
rateCommandPublisher.publish(safeRateCommandPkg);
ROS_INFO_STREAM("RateCommandTimer pubslishes: " << safeRateCommandPkg.rollRate << ", " << safeRateCommandPkg.pitchRate << ", " << safeRateCommandPkg.yawRate);
}
//callback method to publish d_fall_pps::MotorCommand
void callbackMotorCommand(const ros::TimerEvent&)
{
d_fall_pps::MotorCommand motorCommandPkg;
motorCommandPkg.cmd1 = ControlCommandTest.motorCmd1;
motorCommandPkg.cmd2 = ControlCommandTest.motorCmd2;
motorCommandPkg.cmd3 = ControlCommandTest.motorCmd3;
motorCommandPkg.cmd4 = ControlCommandTest.motorCmd4;
//motorCommandPkg.cmd1 = ControlCommandTest.motorCmd1;
//motorCommandPkg.cmd2 = ControlCommandTest.motorCmd2;
//motorCommandPkg.cmd3 = ControlCommandTest.motorCmd3;
//motorCommandPkg.cmd4 = ControlCommandTest.motorCmd4;
motorCommandPublisher.publish(motorCommandPkg);
ROS_INFO_STREAM("MotorCommandTimer pubslishes: " << motorCommandPkg.cmd1 << ", " << motorCommandPkg.cmd2 << ", " << motorCommandPkg.cmd3 << ", " << motorCommandPkg.cmd4);
......@@ -264,9 +277,9 @@ int main(int argc, char* argv[]){
//service
//to be expanded with additional services depending on controller (currently only one available)
rateClient = nodeHandle.serviceClient<d_fall_pps::RateController>("/SafeControllerService/RateController");
safeController = nodeHandle.serviceClient<d_fall_pps::RateController>("/SafeControllerService/RateController");
//rateClient = nodeHandle.serviceClient<d_fall_pps::RateController>("/SafeControllerService/RateController", true);
//safeController = nodeHandle.serviceClient<d_fall_pps::RateController>("/SafeControllerService/RateController", true);
//http://wiki.ros.org/roscpp/Overview/Services
//2.1 Persistenct Connection: ROS also allows for persistent connections to services. With a persistent connection, a client stays connected to a service.
// Otherwise, a client normally does a lookup and reconnects to a service each time.
......
pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
View file @ 2f3e2ce1
......@@ -23,29 +23,65 @@
using namespace d_fall_pps;
/*TODO:
request from paul
Specificy controller in a file
ViconData previousLocation;
*/
bool calculateControlOutput(RateController::Request &request, RateController::Response &response) {
ROS_INFO("calculate control output");
//Philipp: I have put this here, because in the first call, we wouldnt have previousLocation initialized
//save previous data for calculating velocities in next step
previousLocation = request.crazyflieLocation;
ViconData vicon = request.crazyflieLocation;
Setpoint goal = request.setpoint;
//ROS_INFO("request received with following ViconData");
//ROS_INFO_STREAM(vicon);
//ROS_INFO("the goal setpoint is:");
//ROS_INFO_STREAM(request.setpoint);
ROS_INFO("request received with following ViconData");
ROS_INFO_STREAM(vicon);
ROS_INFO("the goal setpoint is:");
ROS_INFO_STREAM(request.setpoint);
//add/calculate safeController
response.controlOutput.rollRate = 1; //testvalue
response.controlOutput.pitchRate = 2; //testvalue
response.controlOutput.yawRate = 3; //testvalue
response.controlOutput.thrust = 50;
//K matrix for kLqrOuter
const float k[] = {
0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0,
1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0,
0, 0, 0, 0, 0, 0, 0, 0, 3.843099534,
0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0
};
float px = request.crazyflieLocation.x - request.setpoint.x;
float py = request.crazyflieLocation.y - request.setpoint.y;
float pz = request.crazyflieLocation.z - request.setpoint.z;
//linear approximation of derivative of position
float vx = (request.crazyflieLocation.x - previousLocation.x) / request.crazyflieLocation.acquiringTime;
float vy = (request.crazyflieLocation.y - previousLocation.y) / request.crazyflieLocation.acquiringTime;
float vz = (request.crazyflieLocation.z - previousLocation.z) / request.crazyflieLocation.acquiringTime;
float roll = request.crazyflieLocation.roll;
float pitch = request.crazyflieLocation.pitch;
float yaw = request.crazyflieLocation.yaw - request.setpoint.yaw;
response.controlOutput.rollRate = -(k[0] * px + k[1] * py + k[2] * pz + k[3] * vx + k[4] * vy + k[5] * vz + k[6] * roll + k[7] * pitch + k[8] * yaw);
response.controlOutput.pitchRate = -(k[9] * px + k[10] * py + k[11] * pz + k[12] * vx + k[13] * vy + k[14] * vz + k[15] * roll + k[16] * pitch + k[17] * yaw);
response.controlOutput.yawRate = -(k[18] * px + k[19] * py + k[20] * pz + k[21] * vx + k[22] * vy + k[23] * vz + k[24] * roll + k[25] * pitch + k[26] * yaw);
float thrustIntermediate = -(k[27] * px + k[28] * py + k[29] * pz + k[30] * vx + k[31] * vy + k[32] * vz + k[33] * roll + k[34] * pitch + k[35] * yaw);
//idea: linerazie plot and apply on sum of thrust instead of on each motor
response.controlOutput.thrust = 20000;
/*cmd1Thrust=(-m_a1+sqrt(m_a1*m_a1-4*m_a2*(m_a0-(thrust+m_ffCmd1Thrust))))/(2*m_a2);
cmd2Thrust=(-m_a1+sqrt(m_a1*m_a1-4*m_a2*(m_a0-(thrust+m_ffCmd2Thrust))))/(2*m_a2);
cmd3Thrust=(-m_a1+sqrt(m_a1*m_a1-4*m_a2*(m_a0-(thrust+m_ffCmd3Thrust))))/(2*m_a2);
cmd4Thrust=(-m_a1+sqrt(m_a1*m_a1-4*m_a2*(m_a0-(thrust+m_ffCmd4Thrust))))/(2*m_a2);
*/
return true;
}
......
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