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