Revision dcf49526
Overwrote the ROS sigint handler so now behaviours stop and die and stop motors upon exiting.
scout/libscout/src/Behavior.cpp | ||
---|---|---|
38 | 38 |
|
39 | 39 |
using namespace std; |
40 | 40 |
|
41 |
bool Behavior::keep_running; |
|
42 |
|
|
41 | 43 |
/** |
42 | 44 |
* Constructs a behavior and sets up all control classes. |
43 | 45 |
* |
... | ... | |
50 | 52 |
*/ |
51 | 53 |
Behavior::Behavior(string scoutname, string my_name, Sensors * sensors) |
52 | 54 |
{ |
53 |
|
|
54 | 55 |
name = my_name; |
56 |
keep_running = true; |
|
55 | 57 |
sensors->init(&motors, &buttons, &sonar, &encoders, &linesensor, |
56 | 58 |
&wl_sender, &wl_receiver); |
57 | 59 |
wl_receiver->register_callback(std::bind(&Behavior::default_callback, |
... | ... | |
66 | 68 |
Behavior::~Behavior() |
67 | 69 |
{ |
68 | 70 |
motors->set_sides(0, 0, MOTOR_ABSOLUTE); |
69 |
ROS_INFO("I get called"); |
|
70 | 71 |
spinOnce(); |
71 |
loop_rate->sleep(); |
|
72 | 72 |
delete wl_receiver; |
73 | 73 |
delete loop_rate; |
74 | 74 |
} |
... | ... | |
86 | 86 |
*/ |
87 | 87 |
bool Behavior::ok() |
88 | 88 |
{ |
89 |
return ros::ok();
|
|
89 |
return keep_running;
|
|
90 | 90 |
} |
91 | 91 |
|
92 | 92 |
/** |
scout/libscout/src/Behavior.h | ||
---|---|---|
39 | 39 |
|
40 | 40 |
#include <ros/ros.h> |
41 | 41 |
#include <std_msgs/String.h> |
42 |
#include <iostream> |
|
42 | 43 |
|
43 | 44 |
#include "MotorControl.h" |
44 | 45 |
#include "HeadlightControl.h" |
... | ... | |
70 | 71 |
|
71 | 72 |
// Name of behaviour |
72 | 73 |
std::string name; |
74 |
|
|
75 |
// Flag to check if sigint recieved. |
|
76 |
static bool keep_running; |
|
73 | 77 |
|
74 | 78 |
protected: |
75 | 79 |
ros::NodeHandle node; |
... | ... | |
91 | 95 |
void default_callback(std::vector<uint8_t> data); |
92 | 96 |
|
93 | 97 |
// Wrappers for ROS functions |
94 |
bool ok(); |
|
95 |
void spin(); |
|
96 |
void spinOnce(); |
|
98 |
static bool ok();
|
|
99 |
static void spin();
|
|
100 |
static void spinOnce();
|
|
97 | 101 |
}; |
98 | 102 |
|
99 | 103 |
#endif |
scout/libscout/src/BehaviorProcess.cpp | ||
---|---|---|
36 | 36 |
|
37 | 37 |
#include "BehaviorProcess.h" |
38 | 38 |
#include "Sensors.h" |
39 |
#include "assert.h" |
|
39 |
#include <assert.h> |
|
40 |
#include <signal.h> |
|
41 |
|
|
42 |
// Make the behavior's ok() function return false. |
|
43 |
void sigint_handler(int sig) |
|
44 |
{ |
|
45 |
Behavior::keep_running = false; |
|
46 |
} |
|
40 | 47 |
|
41 | 48 |
int main (int argc, char **argv) |
42 | 49 |
{ |
... | ... | |
58 | 65 |
scoutname = argv[1]; |
59 | 66 |
behavior_num = atoi(argv[2]); |
60 | 67 |
|
61 |
ros::init(argc, argv, scoutname + "_behavior"); |
|
68 |
ros::init(argc, argv, scoutname + "_behavior", |
|
69 |
ros::init_options::NoSigintHandler); |
|
70 |
|
|
71 |
// Initialize the signal handler after initializing rosnode. |
|
72 |
// Otherwise it will be overwritten and you will be sad. |
|
73 |
signal(SIGINT, sigint_handler); |
|
74 |
|
|
62 | 75 |
// one Sensor instance per-class |
63 | 76 |
Sensors* sensors = new Sensors(scoutname); |
64 | 77 |
BehaviorList* list = new BehaviorList(); |
scout/libscout/src/BehaviorProcess.h | ||
---|---|---|
6 | 6 |
|
7 | 7 |
using namespace std; |
8 | 8 |
|
9 |
int main(int argc, char** argv);
|
|
9 |
int main(int argc, char** argv); |
|
10 | 10 |
|
11 | 11 |
#endif |
scout/libscout/src/behaviors/navigationMap.cpp | ||
---|---|---|
191 | 191 |
|
192 | 192 |
ROS_INFO("Worst case time to 16 is %d", get_worst_case_time(curr_state, 6).sec); |
193 | 193 |
|
194 |
for(int i=0; i<path.len; i++) |
|
194 |
for(int i=0; i<path.len && ok(); i++)
|
|
195 | 195 |
{ |
196 | 196 |
update_state(path.path[i]); |
197 | 197 |
ROS_INFO("Made turn %d, at state %d\n", path.path[i], curr_state); |
scout/libscout/src/behaviors/pause_scout.cpp | ||
---|---|---|
28 | 28 |
void pause_scout::run() |
29 | 29 |
{ |
30 | 30 |
Duration delay(0.2); |
31 |
for(int i=0; i < 500; i++) |
|
31 |
for(int i=0; i < 500 && ok(); i++)
|
|
32 | 32 |
{ |
33 | 33 |
motors->set_sides(0, 0, MOTOR_ABSOLUTE); |
34 | 34 |
delay.sleep(); |
Also available in: Unified diff