Commit a1d7466c authored by roangel's avatar roangel
Browse files

Linear tray following with safe controller, some bugs solved

parent 789e11b4
......@@ -68,13 +68,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext();
// we now have the m_context variable with the current context. Put CF Name in label
QString qstr = "StudentID ";
qstr.append(QString::number(m_student_id));
qstr.append(" connected to CF ");
qstr.append(QString::fromStdString(m_context.crazyflieName));
ui->groupBox->setTitle(qstr);
std::vector<float> default_setpoint(4);
ros::NodeHandle nh_safeControllerService(m_ros_namespace + "/SafeControllerService");
......@@ -250,6 +244,13 @@ void MainWindow::loadCrazyflieContext()
{
m_context = contextCall.response.crazyflieContext;
ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
// we now have the m_context variable with the current context. Put CF Name in label
QString qstr = "StudentID ";
qstr.append(QString::number(m_student_id));
qstr.append(" connected to CF ");
qstr.append(QString::fromStdString(m_context.crazyflieName));
ui->groupBox->setTitle(qstr);
}
else
{
......
2,PPS_CF1,0/0/2M,0,-0.98245,-0.431663,-0.2,0.622035,0.591578,2
5,PPS_CF08,0/56/2M,1,0.165581,-0.217325,-0.2,0.755462,0.548486,2
6,PPS_CF04,0/24/2M,2,-0.476044,-0.910694,-0.2,0.051744,-0.217325,2
4,PPS_CF02,0/8/2M,0,-0.486393,-0.217325,-0.2,0.068015,0.337808,2
3,PPS_CF03,0/16/2M,3,0.0931391,-1.12802,-0.2,0.765811,-0.351859,2
......@@ -61,6 +61,12 @@
#define DURATION_TAKE_OFF 3 // seconds
#define DURATION_LANDING 3 // seconds
// line trayectory defines
#define DISTANCE_THRESHOLD 0.5
#define PI 3.141592653589
using namespace d_fall_pps;
......@@ -79,6 +85,13 @@ float angleMargin;
Setpoint controller_setpoint;
// variables for linear trayectory
Setpoint current_safe_setpoint;
double distance;
double unit_vector[3];
ros::ServiceClient centralManager;
ros::Publisher controlCommandPublisher;
......@@ -116,6 +129,13 @@ float landing_distance;
float duration_take_off;
float duration_landing;
bool finished_take_off = false;
bool finished_land = false;
ros::Timer timer_takeoff;
ros::Timer timer_land;
void loadSafeController() {
ros::NodeHandle nodeHandle("~");
......@@ -194,6 +214,24 @@ void coordinatesToLocal(CrazyflieData& cf) {
}
void setCurrentSafeSetpoint(Setpoint setpoint)
{
current_safe_setpoint = setpoint;
}
void calculateDistanceToCurrentSafeSetpoint(CrazyflieData& local)
{
double dx = current_safe_setpoint.x - local.x;
double dy = current_safe_setpoint.y - local.y;
double dz = current_safe_setpoint.z - local.z;
distance = sqrt(pow(dx,2) + pow(dy,2) + pow(dz,2));
unit_vector[0] = dx/distance;
unit_vector[1] = dy/distance;
unit_vector[2] = dz/distance;
}
void takeOffCF(CrazyflieData& current_local_coordinates) //local because the setpoint is in local coordinates
{
......@@ -215,6 +253,9 @@ void takeOffCF(CrazyflieData& current_local_coordinates) //local because the set
usingSafeController = true;
loadSafeController();
// when do we finish? after some time with delay?
// update variable that keeps track of current setpoint
setCurrentSafeSetpoint(setpoint_msg);
}
void landCF(CrazyflieData& current_local_coordinates)
......@@ -230,6 +271,7 @@ void landCF(CrazyflieData& current_local_coordinates)
// now, use safe controller to go to that setpoint
usingSafeController = true;
loadSafeController();
setCurrentSafeSetpoint(setpoint_msg);
}
void changeFlyingStateTo(int new_state)
......@@ -251,11 +293,6 @@ void changeFlyingStateTo(int new_state)
flyingStatePublisher.publish(flying_state_msg);
}
bool finished_take_off = false;
bool finished_land = false;
ros::Timer timer_takeoff;
ros::Timer timer_land;
void takeOffTimerCallback(const ros::TimerEvent&)
{
......@@ -269,24 +306,11 @@ void landTimerCallback(const ros::TimerEvent&)
void goToControllerSetpoint()
{
// std::vector<float> default_setpoint(4);
// ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
// ROS_INFO_STREAM(ros_namespace << "/SafeControllerService");
// if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
// {
// ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
// }
// Setpoint setpoint_msg;
// setpoint_msg.y = default_setpoint[1];
// setpoint_msg.z = default_setpoint[2];
// ROS_INFO_STREAM("Z =" << default_setpoint[2]);
// setpoint_msg.yaw = default_setpoint[3];
safeControllerServiceSetpointPublisher.publish(controller_setpoint);
setCurrentSafeSetpoint(controller_setpoint);
}
//is called when new data from Vicon arrives
void viconCallback(const ViconData& viconData) {
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
......@@ -376,6 +400,26 @@ void viconCallback(const ViconData& viconData) {
}
else
{
calculateDistanceToCurrentSafeSetpoint(local); // update distance, it also updates the unit vector
// here, detect if euclidean distance between setpoint and current position is higher than a threshold
if(distance > DISTANCE_THRESHOLD)
{
Setpoint setpoint_msg;
// here, where we are now, or where we were in the beginning?
setpoint_msg.x = local.x + DISTANCE_THRESHOLD * unit_vector[0];
setpoint_msg.y = local.y + DISTANCE_THRESHOLD * unit_vector[1];
setpoint_msg.z = local.z + DISTANCE_THRESHOLD * unit_vector[2];
// yaw is better divided by the number of steps?
setpoint_msg.yaw = current_safe_setpoint.yaw;
safeControllerServiceSetpointPublisher.publish(setpoint_msg);
}
else
{
goToControllerSetpoint(); //maybe this is a bit repetitive?
}
bool success = safeController.call(controllerCall);
if(!success) {
ROS_ERROR_STREAM("Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
......@@ -452,31 +496,33 @@ void loadCrazyflieContext() {
if(centralManager.call(contextCall)) {
new_context = contextCall.response.crazyflieContext;
ROS_INFO_STREAM("CrazyflieContext:\n" << new_context);
} else {
ROS_ERROR("Failed to load context");
}
if((context.crazyflieName != "") && (new_context.crazyflieName != context.crazyflieName)) //linked crazyflie name changed and it was not empty before
{
if((context.crazyflieName != "") && (new_context.crazyflieName != context.crazyflieName)) //linked crazyflie name changed and it was not empty before
{
// Motors off is done in python script now everytime we disconnect
// Motors off is done in python script now everytime we disconnect
// send motors OFF and disconnect before setting context = new_context
// std_msgs::Int32 msg;
// msg.data = CMD_CRAZYFLY_MOTORS_OFF;
// commandPublisher.publish(msg);
// send motors OFF and disconnect before setting context = new_context
// std_msgs::Int32 msg;
// msg.data = CMD_CRAZYFLY_MOTORS_OFF;
// commandPublisher.publish(msg);
ROS_INFO("CF is now different for this student. Disconnect and turn it off");
ROS_INFO("CF is now different for this student. Disconnect and turn it off");
std_msgs::Int32 msg;
msg.data = CMD_DISCONNECT;
crazyRadioCommandPublisher.publish(msg);
}
std_msgs::Int32 msg;
msg.data = CMD_DISCONNECT;
crazyRadioCommandPublisher.publish(msg);
}
context = new_context;
context = new_context;
ros::NodeHandle nh("CrazyRadio");
nh.setParam("crazyFlieAddress", context.crazyflieAddress);
ros::NodeHandle nh("CrazyRadio");
nh.setParam("crazyFlieAddress", context.crazyflieAddress);
}
else
{
ROS_ERROR("Failed to load context. Waiting for next Save in DB by teacher");
}
}
......@@ -542,6 +588,10 @@ void controllerSetPointCallback(const Setpoint& newSetpoint)
}
}
void safeSetPointCallback(const Setpoint& newSetpoint)
{
}
void safeYAMLloadedCallback(const std_msgs::Int32& msg)
{
......@@ -608,6 +658,7 @@ int main(int argc, char* argv[])
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
safeControllerServiceSetpointPublisher = namespaceNodeHandle.advertise<d_fall_pps::Setpoint>("SafeControllerService/Setpoint", 1);
ros::Subscriber controllerSetpointSubscriber = namespaceNodeHandle.subscribe("student_GUI/ControllerSetpoint", 1, controllerSetPointCallback);
ros::Subscriber safeSetpointSubscriber = namespaceNodeHandle.subscribe("SafeControllerService/Setpoint", 1, safeSetPointCallback);
// subscriber for DBChanged
ros::Subscriber DBChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, DBChangedCallback);
......
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