Revision 751d4f4f

View differences:

scout/libscout/src/behaviors/Odometry.cpp
4 4
using namespace std;
5 5

  
6 6
/** Set up the odometry node and prepare communcations over ROS */
7
Odometry::Odometry(string scoutname, Sensors* sensors):Behavior(scoutname, 
8
                                                "odometry", sensors)
7
Odometry::Odometry(string scoutname, Sensors* sensors)
8
    : Behavior(scoutname, "odometry", sensors)
9
{
10
  name = scoutname;
11
  scout_pos = new pos;
12
  motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
13
}
14

  
15
/** Set up the odometry node and prepare communcations over ROS */
16
Odometry::Odometry(string scoutname, string behavior_name, Sensors* sensors)
17
    : Behavior(scoutname, behavior_name, sensors)
9 18
{
10 19
  name = scoutname;
11 20
  scout_pos = new pos;
scout/libscout/src/behaviors/Odometry.h
12 12
#define ENCODER_COUNT (2*WHEEL_RADIUS*350*M_PI/WHEEL_BASE)
13 13
#define DIST_PER_TICK (WHEEL_CIRCUM/ENCODER_COUNT)
14 14

  
15
class Odometry : Behavior{
15
class Odometry : public Behavior {
16 16

  
17
  public:
17
    public:
18 18

  
19
  /** Set up the odometry node and prepare communcations over ROS */
20
  Odometry(std::string scoutname, Sensors* sensors);
19
        /** Set up the odometry node and prepare communcations over ROS */
20
        Odometry(std::string scoutname, Sensors* sensors);
21 21

  
22
  /** Query encoders and estimate position based on encoder reading */
23
  void get_position();
22
        /** Set up the odometry node and prepare communcations over ROS */
23
        Odometry(std::string scoutname, std::string behavior_name, Sensors* sensors);
24 24

  
25
  /** Gets scout position and prints to screen */
26
  void run();
27
  
28
  private:
25
        /** Query encoders and estimate position based on encoder reading */
26
        void get_position();
29 27

  
30
    /** ROS publisher and client declaration */
31
    ros::NodeHandle node;
32
    ros::Publisher scout_position;
33
    ros::ServiceClient query_encoders_client;
34
    messages::ScoutPosition position;
28
        /** Gets scout position and prints to screen */
29
        void run();
35 30

  
36
    std::string name;
31
    protected:
37 32

  
38
    float msg_time_in;
33
        pos* scout_pos;
39 34

  
40
    int motor_fl_dist;
41
    int motor_fr_dist;
42
    int motor_bl_dist;
43
    int motor_br_dist;
35
    private:
44 36

  
45
    unsigned int motor_fl_ticks;
46
    unsigned int motor_fr_ticks;
47
    unsigned int motor_bl_ticks;
48
    unsigned int motor_br_ticks;
49
    float scout_theta;
37
        /** ROS publisher and client declaration */
38
        ros::NodeHandle node;
39
        ros::Publisher scout_position;
40
        ros::ServiceClient query_encoders_client;
41
        messages::ScoutPosition position;
50 42

  
51
    pos* scout_pos;
43
        std::string name;
44

  
45
        float msg_time_in;
46

  
47
        int motor_fl_dist;
48
        int motor_fr_dist;
49
        int motor_bl_dist;
50
        int motor_br_dist;
51

  
52
        unsigned int motor_fl_ticks;
53
        unsigned int motor_fr_ticks;
54
        unsigned int motor_bl_ticks;
55
        unsigned int motor_br_ticks;
56
        float scout_theta;
52 57
};
53 58

  
54 59
#endif
scout/libscout/src/test_behaviors/danger_marking.cpp
37 37
    // Turn the sonar on.
38 38
    sonar->set_on();
39 39
    wait(1.5);
40
    sonar->set_single(36);
40
    sonar->set_range(35, 37);
41 41

  
42 42
    motors->set_sides(-20, -20);
43 43

  
......
45 45

  
46 46
    while (true)
47 47
    {
48
        get_position();
49
        ROS_INFO("Position: (%f, %f, %f).",
50
            scout_pos->x, scout_pos->y, scout_pos->theta);
51

  
48 52
        int *readings = sonar->get_sonar_readings();
49 53
        dist = min(readings[35], min(readings[36], readings[37]));
50 54

  
scout/libscout/src/test_behaviors/danger_marking.h
27 27
#define _DANGER_MARKING_H_
28 28

  
29 29
#include <stdio.h>
30
#include "../Behavior.h"
31
#include "../Sensors.h"
30
#include "../behaviors/Odometry.h"
32 31

  
33 32
/**
34 33
 * A very simple maze solver that always turns right.
35 34
 */
36
class danger_marking : public Behavior
35
class danger_marking : public Odometry
37 36
{
38 37
    public:
39 38
        danger_marking(std::string scoutname, Sensors* sensors):
40
            Behavior(scoutname, "danger_marking", sensors) {};
39
            Odometry(scoutname, "danger_marking", sensors) {};
41 40
        void run();
42 41
};
43 42
#endif

Also available in: Unified diff