Revision dcf49526

View differences:

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