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