Revision 751d4f4f
Made danger_marking depend on Odometry correctly
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