PPSClient.cpp 29.2 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//    ROS node that manages the student's setup.
//    Copyright (C) 2017  Cyrill Burgener, Marco Mueller, Philipp Friedli
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include "ros/ros.h"
muelmarc's avatar
muelmarc committed
18
#include <stdlib.h>
19
#include <std_msgs/String.h>
20
#include <rosbag/bag.h>
muelmarc's avatar
muelmarc committed
21
#include <ros/package.h>
22

23
#include "d_fall_pps/Controller.h"
24
#include "d_fall_pps/CMQuery.h"
25
26

#include "d_fall_pps/ViconData.h"
27
#include "d_fall_pps/CrazyflieData.h"
28
#include "d_fall_pps/ControlCommand.h"
muelmarc's avatar
muelmarc committed
29
#include "d_fall_pps/CrazyflieContext.h"
30
#include "d_fall_pps/Setpoint.h"
31
#include "std_msgs/Int32.h"
32
#include "std_msgs/Float32.h"
33

muelmarc's avatar
muelmarc committed
34

35
#include "d_fall_pps/ControlCommand.h"
36

37
38
39
40
// tipes of controllers being used:
#define SAFE_CONTROLLER   0
#define CUSTOM_CONTROLLER 1

41
#define CMD_USE_SAFE_CONTROLLER   1
42
#define CMD_USE_CUSTOM_CONTROLLER 2
43
44
45
46
47
48
49
50
51
52
#define CMD_CRAZYFLY_TAKE_OFF     3
#define CMD_CRAZYFLY_LAND         4
#define CMD_CRAZYFLY_MOTORS_OFF   5

// Flying states
#define STATE_MOTORS_OFF 1
#define STATE_TAKE_OFF   2
#define STATE_FLYING     3
#define STATE_LAND       4

53
54
55
56
57
// battery states

#define BATTERY_STATE_NORMAL 0
#define BATTERY_STATE_LOW    1

58
59
60
61
62
63
64
65
66
67
68
// commands for CrazyRadio
#define CMD_RECONNECT  0
#define CMD_DISCONNECT 1


// CrazyRadio states:
#define CONNECTED        0
#define CONNECTING       1
#define DISCONNECTED     2

// parameters for take off and landing. Eventually will go in YAML file
69
70
#define TAKE_OFF_OFFSET  1    //in meters
#define LANDING_DISTANCE 0.15    //in meters
roangel's avatar
roangel committed
71
72
#define DURATION_TAKE_OFF  3   // seconds
#define DURATION_LANDING   3   // seconds
73

74

75
76
#define PI 3.141592653589

77
using namespace d_fall_pps;
78

79
80
//studentID, gives namespace and identifier in CentralManagerService
int studentID;
81

82
//the safe controller specified in the ClientConfig.yaml, is considered stable
83
ros::ServiceClient safeController;
84
//the custom controller specified in the ClientConfig.yaml, is considered potentially unstable
85
ros::ServiceClient customController;
86

87
88
89
//values for safteyCheck
bool strictSafety;
float angleMargin;
90

91
92
93
94
95
96
97
98
99
100
// battery threshold
float m_battery_threshold_while_flying;
float m_battery_threshold_while_motors_off;


// battery values

int m_battery_state;
float m_battery_voltage;

101
102
Setpoint controller_setpoint;

103
104
105
106
// variables for linear trayectory
Setpoint current_safe_setpoint;
double distance;
double unit_vector[3];
roangel's avatar
roangel committed
107
bool was_in_threshold = false;
108
double distance_threshold;      //to be loaded from yaml
109
110


111
ros::ServiceClient centralManager;
112
ros::Publisher controlCommandPublisher;
113

114
115
116
// communicate with safeControllerService, setpoint, etc...
ros::Publisher safeControllerServiceSetpointPublisher;

117
118
119
// publisher for flying state
ros::Publisher flyingStatePublisher;

120
121
122
// publisher for battery state
ros::Publisher batteryStatePublisher;

123
124
125
126
127
128
// publisher to send commands to itself.
ros::Publisher commandPublisher;

// communication with crazyRadio node. Connect and disconnect
ros::Publisher crazyRadioCommandPublisher;

129
130
rosbag::Bag bag;

131
132
133
134
// variables for the states:
int flying_state;
bool changed_state_flag;

135
136
137
// variable for crazyradio status
int crazyradio_status;

138
139
//describes the area of the crazyflie and other parameters
CrazyflieContext context;
140

141
//wheter to use safe of custom controller
142
143
144
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:
145
int controller_used;
146
147

ros::Publisher controllerUsedPublisher;
148

149
150
std::string ros_namespace;

151
152
153
154
155
float take_off_distance;
float landing_distance;
float duration_take_off;
float duration_landing;

156
157
158
159
160
161
162
bool finished_take_off = false;
bool finished_land = false;

ros::Timer timer_takeoff;
ros::Timer timer_land;


163
164
165
166
167
168
169
170
171
172
173
174
175
176
void loadSafeController() {
	ros::NodeHandle nodeHandle("~");

	std::string safeControllerName;
	if(!nodeHandle.getParam("safeController", safeControllerName)) {
		ROS_ERROR("Failed to get safe controller name");
		return;
	}

	ros::service::waitForService(safeControllerName);
	safeController = ros::service::createClient<Controller>(safeControllerName, true);
    ROS_INFO_STREAM("loaded safe controller: " << safeController.getService());
}

177
178
void loadCustomController()
{
179
180
181
	ros::NodeHandle nodeHandle("~");

	std::string customControllerName;
182
183
	if(!nodeHandle.getParam("customController", customControllerName))
    {
184
185
186
187
		ROS_ERROR("Failed to get custom controller name");
		return;
	}

188
    customController = ros::service::createClient<Controller>(customControllerName, true);
189
190
191
    ROS_INFO_STREAM("loaded custom controller " << customControllerName);
}

192

193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
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;
roangel's avatar
roangel committed
226
227
228
229
230

    if(flying_state == STATE_MOTORS_OFF || flying_state == STATE_FLYING)
    {
        setInstantController(controller); //if motors OFF or STATE FLYING, transparent, change is instant
    }
231
232
233
234
235
236
237
238
239
}

int getControllerUsed()
{
    return controller_used;
}



240
//checks if crazyflie is within allowed area and if custom controller returns no data
241
bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
242
	//position check
243
	if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
244
		ROS_INFO_STREAM("x safety failed");
245
		return false;
muelmarc's avatar
muelmarc committed
246
	}
247
	if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
248
		ROS_INFO_STREAM("y safety failed");
249
		return false;
muelmarc's avatar
muelmarc committed
250
	}
251
	if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) {
252
		ROS_INFO_STREAM("z safety failed");
253
		return false;
muelmarc's avatar
muelmarc committed
254
	}
255

256
257
258
259
260
261
262
263
264
265
266
267
268
	//attitude check
	//if strictSafety is set to true in ClientConfig.yaml the SafeController takes also over if the roll and pitch angles get to large
	//the angleMargin is a value in the range (0,1). The closer to 1, the closer to 90 deg are the roll and pitch angles allowed to become before the safeController takes over
	if(strictSafety){
		if((data.roll > PI/2*angleMargin) or (data.roll < -PI/2*angleMargin)) {
			ROS_INFO_STREAM("roll too big.");
			return false;
		}
		if((data.pitch > PI/2*angleMargin) or (data.pitch < -PI/2*angleMargin)) {
			ROS_INFO_STREAM("pitch too big.");
			return false;
		}
	}
269

270
	return true;
271
}
phfriedl's avatar
phfriedl committed
272

bucyril's avatar
bucyril committed
273
274
275
276
void coordinatesToLocal(CrazyflieData& cf) {
	AreaBounds area = context.localArea;
	float originX = (area.xmin + area.xmax) / 2.0;
	float originY = (area.ymin + area.ymax) / 2.0;
277
278
279
    // change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
    float originZ = 0.0;
	// float originZ = (area.zmin + area.zmax) / 2.0;
bucyril's avatar
bucyril committed
280
281
282
283
284
285

	cf.x -= originX;
	cf.y -= originY;
	cf.z -= originZ;
}

286

287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
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;
}

305
306
307
308
309
310
311

void takeOffCF(CrazyflieData& current_local_coordinates) //local because the setpoint is in local coordinates
{
    // set the setpoint and call safe controller
    Setpoint setpoint_msg;
    setpoint_msg.x = current_local_coordinates.x;           // previous one
    setpoint_msg.y = current_local_coordinates.y;           //previous one
312
    setpoint_msg.z = current_local_coordinates.z + take_off_distance;           //previous one plus some offset
313
314
    // setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
    setpoint_msg.yaw = 0.0;
315
    safeControllerServiceSetpointPublisher.publish(setpoint_msg);
316
317
318
319
320
    ROS_INFO("Take OFF:");
    ROS_INFO("------Current coordinates:");
    ROS_INFO("X: %f, Y: %f, Z: %f", current_local_coordinates.x, current_local_coordinates.y, current_local_coordinates.z);
    ROS_INFO("------New coordinates:");
    ROS_INFO("X: %f, Y: %f, Z: %f", setpoint_msg.x, setpoint_msg.y, setpoint_msg.z);
321
322
323

    // now, use safe controller to go to that setpoint
    loadSafeController();
324
    setInstantController(SAFE_CONTROLLER);
325
    // when do we finish? after some time with delay?
326
327
328

    // update variable that keeps track of current setpoint
    setCurrentSafeSetpoint(setpoint_msg);
329
330
331
332
333
334
335
336
}

void landCF(CrazyflieData& current_local_coordinates)
{
    // set the setpoint and call safe controller
    Setpoint setpoint_msg;
    setpoint_msg.x = current_local_coordinates.x;           // previous one
    setpoint_msg.y = current_local_coordinates.y;           //previous one
337
    setpoint_msg.z = landing_distance;           //previous one plus some offset
338
339
340
341
342
    setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
    safeControllerServiceSetpointPublisher.publish(setpoint_msg);

    // now, use safe controller to go to that setpoint
    loadSafeController();
343
    setInstantController(SAFE_CONTROLLER);
344
    setCurrentSafeSetpoint(setpoint_msg);
345
346
347
348
}

void changeFlyingStateTo(int new_state)
{
349
350
351
352
353
354
355
356
357
358
359
    if(crazyradio_status == CONNECTED)
    {
        ROS_INFO("Change state to: %d", new_state);
        flying_state = new_state;
    }
    else
    {
        ROS_INFO("Disconnected and trying to change state. Stays goes to MOTORS OFF");
        flying_state = STATE_MOTORS_OFF;
    }

360
    changed_state_flag = true;
361
362
363
    std_msgs::Int32 flying_state_msg;
    flying_state_msg.data = flying_state;
    flyingStatePublisher.publish(flying_state_msg);
364
365
}

roangel's avatar
roangel committed
366
367
368
369
370
371
372
373
374
375
376

void takeOffTimerCallback(const ros::TimerEvent&)
{
    finished_take_off = true;
}

void landTimerCallback(const ros::TimerEvent&)
{
    finished_land = true;
}

377
void goToControllerSetpoint()
378
{
379
    safeControllerServiceSetpointPublisher.publish(controller_setpoint);
380
    setCurrentSafeSetpoint(controller_setpoint);
381
382
}

383

384
//is called when new data from Vicon arrives
bucyril's avatar
bucyril committed
385
void viconCallback(const ViconData& viconData) {
386
	for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
bucyril's avatar
bucyril committed
387
		CrazyflieData global = *it;
388

389
        if(global.crazyflieName == context.crazyflieName)
390
        {
391
392
393
394
395
            Controller controllerCall;
            CrazyflieData local = global;
            coordinatesToLocal(local);
            controllerCall.request.ownCrazyflie = local;

roangel's avatar
roangel committed
396
397
            ros::NodeHandle nodeHandle("~");

398
399
400
401
402
403
            switch(flying_state) //things here repeat every X ms, non-blocking stuff
            {
                case STATE_MOTORS_OFF: // here we will put the code of current disabled button
                    if(changed_state_flag) // stuff that will be run only once when changing state
                    {
                        changed_state_flag = false;
404
                        ROS_INFO("STATE_MOTORS_OFF");
405
406
407
408
409
410
411
412
                    }
                    break;
                case STATE_TAKE_OFF: //we need to move up from where we are now.
                    if(changed_state_flag) // stuff that will be run only once when changing state
                    {
                        changed_state_flag = false;
                        takeOffCF(local);
                        finished_take_off = false;
413
                        ROS_INFO("STATE_TAKE_OFF");
414
                        timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_take_off), takeOffTimerCallback, true);
415
416
417
418
                    }
                    if(finished_take_off)
                    {
                        finished_take_off = false;
419
                        setInstantController(getControllerUsed());
420
421
422
423
424
425
426
                        changeFlyingStateTo(STATE_FLYING);
                    }
                    break;
                case STATE_FLYING:
                    if(changed_state_flag) // stuff that will be run only once when changing state
                    {
                        changed_state_flag = false;
427
428
                        // need to change setpoint to the controller one
                        goToControllerSetpoint();
429
                        ROS_INFO("STATE_FLYING");
430
431
432
433
434
435
436
437
                    }
                    break;
                case STATE_LAND:
                    if(changed_state_flag) // stuff that will be run only once when changing state
                    {
                        changed_state_flag = false;
                        landCF(local);
                        finished_land = false;
438
                        ROS_INFO("STATE_LAND");
439
                        timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_landing), landTimerCallback, true);
440
441
442
443
                    }
                    if(finished_land)
                    {
                        finished_land = false;
444
                        setInstantController(getControllerUsed());
445
446
447
448
449
450
                        changeFlyingStateTo(STATE_MOTORS_OFF);
                    }
                    break;
                default:
                    break;
            }
451

452
453
454
455
456
            // 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.
                {
457
                    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
458
                    {
459
                        bool success = customController.call(controllerCall);
460
461
                        if(!success)
                        {
462
463
464
                            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());
465
                            setInstantController(SAFE_CONTROLLER);
466
467
468
                        }
                        else if(!safetyCheck(global, controllerCall.response.controlOutput))
                        {
469
                            setInstantController(SAFE_CONTROLLER);
470
471
472
                            ROS_INFO_STREAM("safety check failed, switching to safe controller");
                        }
                    }
473
                    else        //SAFE_CONTROLLER and state is different from flying
474
                    {
475
                        calculateDistanceToCurrentSafeSetpoint(local); // update distance, it also updates the unit vector
roangel's avatar
roangel committed
476
                        // ROS_INFO_STREAM("distance: " << distance);
477
                        // here, detect if euclidean distance between setpoint and current position is higher than a threshold
478
                        if(distance > distance_threshold)
479
                        {
480
                            ROS_INFO("inside threshold");
481
482
                            Setpoint setpoint_msg;
                            // here, where we are now, or where we were in the beginning?
483
484
485
                            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];
486
487
488
489

                            // yaw is better divided by the number of steps?
                            setpoint_msg.yaw = current_safe_setpoint.yaw;
                            safeControllerServiceSetpointPublisher.publish(setpoint_msg);
roangel's avatar
roangel committed
490
                            was_in_threshold = true;
491
492
493
                        }
                        else
                        {
roangel's avatar
roangel committed
494
495
496
497
498
499
                            if(was_in_threshold)
                            {
                                was_in_threshold = false;
                                safeControllerServiceSetpointPublisher.publish(current_safe_setpoint);
                                // goToControllerSetpoint(); //maybe this is a bit repetitive?
                            }
500
501
                        }

502
503
504
505
506
507
508
509
510
511
512
513
514
                        bool success = safeController.call(controllerCall);
                        if(!success) {
                            ROS_ERROR_STREAM("Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
                        }
                    }


                    controlCommandPublisher.publish(controllerCall.response.controlOutput);

                    bag.write("ViconData", ros::Time::now(), local);
                    bag.write("ControlOutput", ros::Time::now(), controllerCall.response.controlOutput);
                }
            }
515
516
517
518
519
520
521
522
            else
            {
                ControlCommand zeroOutput = ControlCommand(); //everything set to zero
                zeroOutput.onboardControllerType = 2; //set to motor_mode
                controlCommandPublisher.publish(zeroOutput);
                bag.write("ViconData", ros::Time::now(), local);
                bag.write("ControlOutput", ros::Time::now(), zeroOutput);
            }
523
        }
524
525
526
527
	}
}

void loadParameters(ros::NodeHandle& nodeHandle) {
528
529
530
	if(!nodeHandle.getParam("studentID", studentID)) {
		ROS_ERROR("Failed to get studentID");
	}
531
532
533
534
535
536
537
538
	if(!nodeHandle.getParam("strictSafety", strictSafety)) {
		ROS_ERROR("Failed to get strictSafety param");
		return;
	}
	if(!nodeHandle.getParam("angleMargin", angleMargin)) {
		ROS_ERROR("Failed to get angleMargin param");
		return;
	}
539
540
541
542
543
544
545
546
    if(!nodeHandle.getParam("battery_threshold_while_flying", m_battery_threshold_while_flying)) {
		ROS_ERROR("Failed to get battery_threshold_while_flying param");
		return;
	}
    if(!nodeHandle.getParam("battery_threshold_while_motors_off", m_battery_threshold_while_motors_off)) {
		ROS_ERROR("Failed to get battery_threshold_while_motors_off param");
		return;
	}
547
548
549
550
551
552
553
554
555
}

void loadSafeControllerParameters()
{
    ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
    if(!nh_safeControllerService.getParam("takeOffDistance", take_off_distance))
    {
		ROS_ERROR("Failed to get takeOffDistance");
	}
556

557
558
559
560
    if(!nh_safeControllerService.getParam("landingDistance", landing_distance))
    {
		ROS_ERROR("Failed to get landing_distance");
	}
561

562
563
564
565
566
567
568
569
570
    if(!nh_safeControllerService.getParam("durationTakeOff", duration_take_off))
    {
		ROS_ERROR("Failed to get duration_take_off");
	}

    if(!nh_safeControllerService.getParam("durationLanding", duration_landing))
    {
		ROS_ERROR("Failed to get duration_landing");
	}
571
572
573
574
575

    if(!nh_safeControllerService.getParam("distanceThreshold", distance_threshold))
    {
		ROS_ERROR("Failed to get distance_threshold");
	}
576
577
}

578
void loadCrazyflieContext() {
579
580
	CMQuery contextCall;
	contextCall.request.studentID = studentID;
Cyrill Burgener's avatar
Cyrill Burgener committed
581
	ROS_INFO_STREAM("StudentID:" << studentID);
582

583
584
    CrazyflieContext new_context;

585
586
587
	centralManager.waitForExistence(ros::Duration(-1));

	if(centralManager.call(contextCall)) {
588
589
		new_context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("CrazyflieContext:\n" << new_context);
590

591
592
        if((context.crazyflieName != "") && (new_context.crazyflieName != context.crazyflieName)) //linked crazyflie name changed and it was not empty before
        {
593

594
            // Motors off is done in python script now everytime we disconnect
595

596
597
598
599
            // send motors OFF and disconnect before setting context = new_context
            // std_msgs::Int32 msg;
            // msg.data = CMD_CRAZYFLY_MOTORS_OFF;
            // commandPublisher.publish(msg);
600

601
            ROS_INFO("CF is now different for this student. Disconnect and turn it off");
602

603
604
605
606
            std_msgs::Int32 msg;
            msg.data = CMD_DISCONNECT;
            crazyRadioCommandPublisher.publish(msg);
        }
607

608
        context = new_context;
609

610
611
612
613
614
615
616
        ros::NodeHandle nh("CrazyRadio");
        nh.setParam("crazyFlieAddress", context.crazyflieAddress);
	}
    else
    {
		ROS_ERROR("Failed to load context. Waiting for next Save in DB by teacher");
	}
617
}
618

619

620
621
622

void commandCallback(const std_msgs::Int32& commandMsg) {
	int cmd = commandMsg.data;
623

624
625
	switch(cmd) {
    	case CMD_USE_SAFE_CONTROLLER:
626
            ROS_INFO("USE_SAFE_CONTROLLER Command received");
627
            setControllerUsed(SAFE_CONTROLLER);
628
629
630
    		break;

    	case CMD_USE_CUSTOM_CONTROLLER:
631
            ROS_INFO("USE_CUSTOM_CONTROLLER Command received");
632
            setControllerUsed(CUSTOM_CONTROLLER);
633
634
    		break;

635
    	case CMD_CRAZYFLY_TAKE_OFF:
636
637
638
639
            if(flying_state == STATE_MOTORS_OFF)
            {
                changeFlyingStateTo(STATE_TAKE_OFF);
            }
640
641
    		break;

642
    	case CMD_CRAZYFLY_LAND:
643
644
645
646
            if(flying_state != STATE_MOTORS_OFF)
            {
                changeFlyingStateTo(STATE_LAND);
            }
647
    		break;
648
649
650
        case CMD_CRAZYFLY_MOTORS_OFF:
            changeFlyingStateTo(STATE_MOTORS_OFF);
            break;
651

652
653
654
    	default:
    		ROS_ERROR_STREAM("unexpected command number: " << cmd);
    		break;
655
	}
656
657
}

658
659
660
661
662
void DBChangedCallback(const std_msgs::Int32& msg)
{
    loadCrazyflieContext();
}

663
664
665
666
667
void emergencyStopCallback(const std_msgs::Int32& msg)
{
    commandCallback(msg);
}

668
669
670
671
672
void crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
    crazyradio_status = msg.data;
}

673
674
675
676
677
678
679
680
681
682
683
684
void controllerSetPointCallback(const Setpoint& newSetpoint)
{
    // load in variable the setpoint
    controller_setpoint = newSetpoint;

    // if we are in flying, set it up NOW
    if(flying_state == STATE_FLYING)
    {
        goToControllerSetpoint();
    }
}

685
686
687
688
void safeSetPointCallback(const Setpoint& newSetpoint)
{
}

689
690
691
692
693
694
695

void safeYAMLloadedCallback(const std_msgs::Int32& msg)
{
    ROS_INFO("received msg safe loaded YAML");
    loadSafeControllerParameters();
}

696
697
698
699
700
701
702
703
704
705
706
707
int getBatteryState()
{
    return m_battery_state;
}


void setBatteryStateTo(int new_battery_state)
{
    switch(new_battery_state)
    {
        case BATTERY_STATE_NORMAL:
            m_battery_state = BATTERY_STATE_NORMAL;
708
            ROS_INFO("changed battery state to normal");
709
710
711
            break;
        case BATTERY_STATE_LOW:
            m_battery_state = BATTERY_STATE_LOW;
712
            ROS_INFO("changed battery state to low");
roangel's avatar
roangel committed
713
714
            if(flying_state != STATE_MOTORS_OFF)
                changeFlyingStateTo(STATE_LAND);
715
716
717
718
719
720
721
722
723
724
725
726
            break;
        default:
            ROS_INFO("Unknown battery state command, set to normal");
            m_battery_state = BATTERY_STATE_NORMAL;
            break;
    }

    std_msgs::Int32 battery_state_msg;
    battery_state_msg.data = getBatteryState();
    batteryStatePublisher.publish(battery_state_msg);
}

727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
float movingAverageBatteryFilter(float new_input)
{
    const int N = 7;
    static float previous_output = 0;
    static float inputs[N];


    // imagine an array of an even number of samples, we will output the one in the middle averaged with information from all of them.
    // for that, we only need to store some past of the signal
    float output = previous_output + new_input/N - inputs[N-1]/N;

    // update array of inputs
    for(int i = N - 1; i > 0; i--)
    {
        inputs[i] = inputs[i-1];
    }
roangel's avatar
roangel committed
743
744
    inputs[0] = new_input;

745
746
747
748
749
750

    // update previous output
    previous_output = output;
    return output;
}

751

752
void CFBatteryCallback(const std_msgs::Float32& msg)
753
754
755
756
{
    m_battery_voltage = msg.data;
    // filter and check if inside limits, and if, change status
    // need to do the filtering first
757
    float filtered_battery_voltage = movingAverageBatteryFilter(m_battery_voltage); //need to perform filtering here
roangel's avatar
roangel committed
758
759

    ROS_INFO_STREAM("filtered data: " << filtered_battery_voltage);
760
761
762
    if((flying_state != STATE_MOTORS_OFF && (filtered_battery_voltage < m_battery_threshold_while_flying)) ||
       (flying_state == STATE_MOTORS_OFF && (filtered_battery_voltage < m_battery_threshold_while_motors_off)))
    {
roangel's avatar
roangel committed
763
764
        if(getBatteryState() != BATTERY_STATE_LOW)
            setBatteryStateTo(BATTERY_STATE_LOW);
765
        ROS_INFO("low level battery triggered");
766
767
768
769
770
771
772
773
    }
    else                        //maybe add hysteresis somewhere here?
    {
        if(getBatteryState() != BATTERY_STATE_NORMAL)
            setBatteryStateTo(BATTERY_STATE_NORMAL);
    }
}

774
775
int main(int argc, char* argv[])
{
776
777
	ros::init(argc, argv, "PPSClient");
	ros::NodeHandle nodeHandle("~");
778
779
    ros_namespace = ros::this_node::getNamespace();

780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
    // load default setpoint
    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'");
    }

    controller_setpoint.x = default_setpoint[0];
    controller_setpoint.y = default_setpoint[1];
    controller_setpoint.z = default_setpoint[2];
    controller_setpoint.yaw = default_setpoint[3];

    // load context parameters
797
	loadParameters(nodeHandle);
798
    loadSafeControllerParameters();
799

800
	//ros::service::waitForService("/CentralManagerService/CentralManager");
801
	centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
802
	loadCrazyflieContext();
803

804
805
	//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
	ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback);
806
	ROS_INFO_STREAM("successfully subscribed to ViconData");
807

808
	//ros::Publishers to advertise the control output
bucyril's avatar
bucyril committed
809
	controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
phfriedl's avatar
phfriedl committed
810

811
	//this topic lets the PPSClient listen to the terminal commands
812
    commandPublisher = nodeHandle.advertise<std_msgs::Int32>("Command", 1);
bucyril's avatar
bucyril committed
813
    ros::Subscriber commandSubscriber = nodeHandle.subscribe("Command", 1, commandCallback);
phfriedl's avatar
phfriedl committed
814

815
    //this topic lets us use the terminal to communicate with crazyRadio node.
816
    crazyRadioCommandPublisher = nodeHandle.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
817

818
819
    // this topic will publish flying state whenever it changes.
    flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
820

821
822
823
    // will publish battery state when it changes
    batteryStatePublisher = nodeHandle.advertise<std_msgs::Int32>("batteryState", 1);

824
825
    controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerUsed", 1);

826
827
    // crazy radio status
    crazyradio_status = DISCONNECTED;
828

829
830
831
832
833
    // publish first flying state data
    std_msgs::Int32 flying_state_msg;
    flying_state_msg.data = flying_state;
    flyingStatePublisher.publish(flying_state_msg);

834
835
836
    // SafeControllerServicePublisher:
    ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
    safeControllerServiceSetpointPublisher = namespaceNodeHandle.advertise<d_fall_pps::Setpoint>("SafeControllerService/Setpoint", 1);
837
    ros::Subscriber controllerSetpointSubscriber = namespaceNodeHandle.subscribe("student_GUI/ControllerSetpoint", 1, controllerSetPointCallback);
838
    ros::Subscriber safeSetpointSubscriber = namespaceNodeHandle.subscribe("SafeControllerService/Setpoint", 1, safeSetPointCallback);
839

840
    // subscriber for DBChanged
841
    ros::Subscriber DBChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, DBChangedCallback);
842

843
844
845
    // subscriber for emergencyStop
    ros::Subscriber emergencyStopSubscriber = namespaceNodeHandle.subscribe("/my_GUI/emergencyStop", 1, emergencyStopCallback);

846
847
848
    // crazyradio status. Connected, connecting or disconnected
    ros::Subscriber crazyRadioStatusSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, crazyRadioStatusCallback);

849
850
    ros::Subscriber safeYAMloadedSubscriber = namespaceNodeHandle.subscribe("student_GUI/safeYAMLloaded", 1, safeYAMLloadedCallback);

851
852
853
    // know the battery level of the CF
    ros::Subscriber CFBatterySubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CFBattery", 1, CFBatteryCallback);

854
	//start with safe controller
855
    flying_state = STATE_MOTORS_OFF;
856
857
    setControllerUsed(SAFE_CONTROLLER);
    setInstantController(SAFE_CONTROLLER); //initialize this also, so we notify GUI
858

859
860
    setBatteryStateTo(BATTERY_STATE_NORMAL); //initialize battery state

muelmarc's avatar
muelmarc committed
861
862
863
	std::string package_path;
	package_path = ros::package::getPath("d_fall_pps") + "/";
	ROS_INFO_STREAM(package_path);
864
	std::string record_file = package_path + "LoggingPPSClient.bag";
muelmarc's avatar
muelmarc committed
865
866
	bag.open(record_file, rosbag::bagmode::Write);

867
    ros::spin();
muelmarc's avatar
muelmarc committed
868
	bag.close();
869
    return 0;
870
}