Revision 1905324e
ID | 1905324ede544b0057161ba7a6ff8f31be9ebc45 |
Trying to make the warehouse robot drive around
scout/libscout/src/behaviors/Scheduler.cpp | ||
---|---|---|
26 | 26 |
*/ |
27 | 27 |
void Scheduler::get_task(int robot) |
28 | 28 |
{ |
29 |
ROS_INFO("SCHEDULER: robots vector size is: %d", robots.size()); |
|
30 | 29 |
Robot my_robot = robots[robot-1]; |
31 |
ROS_INFO("SCHEDULER: My robot is %s", my_robot.name.c_str()); |
|
32 | 30 |
if(my_robot.sched_status != WAITING_ROBOT) |
33 | 31 |
{ |
34 | 32 |
waitingRobots.push(my_robot); |
35 | 33 |
my_robot.sched_status = WAITING_ROBOT; |
36 |
ROS_INFO("SCHEDULER: Added to scheduler %d", my_robot.sched_status); |
|
37 | 34 |
} |
38 | 35 |
} |
39 | 36 |
|
... | ... | |
48 | 45 |
Duration five(5); |
49 | 46 |
Duration three(3); |
50 | 47 |
Duration ten(10); |
51 |
Order a(1,0,1,t+ten,p,end);
|
|
52 |
Order b(2,0,1,t+five,p,end);
|
|
53 |
Order c(3,0,9,t+three,p,end);
|
|
54 |
Order d(4,0,2,t,p,end);
|
|
55 |
Order e(5,0,4,t,p,end);
|
|
48 |
Order a(1,0,2,t+ten,p,end);
|
|
49 |
Order b(2,0,4,t+five,p,end);
|
|
50 |
Order c(3,0,5,t+three,p,end);
|
|
51 |
Order d(4,0,6,t,p,end);
|
|
52 |
Order e(5,0,7,t,p,end);
|
|
56 | 53 |
|
57 | 54 |
unassignedOrders->insert(a); |
58 | 55 |
unassignedOrders->insert(b); |
... | ... | |
168 | 165 |
new_robot.sched_status = NEW_ROBOT; |
169 | 166 |
robots.push_back(new_robot); |
170 | 167 |
|
171 |
ROS_INFO("Registration a robot %s %d", new_robot.name.c_str(), robots.size()); |
|
172 | 168 |
std_msgs::String msg; |
173 | 169 |
std::stringstream ss; |
174 | 170 |
ss<<"REG_SUCCESS "<<id; |
... | ... | |
196 | 192 |
ROS_INFO("I am a scheduler!. Now waiting for robots"); |
197 | 193 |
while(robots.size() < 1 && ok()) |
198 | 194 |
{ |
199 |
ROS_INFO("Robots size: %d", robots.size()); |
|
200 | 195 |
spinOnce(); |
201 | 196 |
} |
202 | 197 |
ROS_INFO("SCHEDULER: main loop"); |
scout/libscout/src/behaviors/WH_Robot.cpp | ||
---|---|---|
2 | 2 |
#include "../helper_classes/Order.h" |
3 | 3 |
|
4 | 4 |
/** @Brief: warehouse robot constructor **/ |
5 |
WH_Robot::WH_Robot(std::string scoutname):Behavior(scoutname, "WH_Robot")
|
|
5 |
WH_Robot::WH_Robot(std::string scoutname):line_follow(scoutname)
|
|
6 | 6 |
{ |
7 | 7 |
nav_map = new navigationMap(scoutname); |
8 | 8 |
curr_task = DEFAULT_TASK; |
... | ... | |
26 | 26 |
* @param the Path to follow |
27 | 27 |
*/ |
28 | 28 |
void WH_Robot::follow_path(Path path_to_follow){ |
29 |
ROS_INFO("Path length: %d", path_to_follow.len); |
|
30 | 29 |
for(int i=0; i<path_to_follow.len; i++) |
31 | 30 |
{ |
32 | 31 |
Turn t = path_to_follow.path[i]; |
33 |
ROS_INFO("making turn %d at state %d", (int)t, (int)nav_map->get_state());
|
|
32 |
ROS_INFO("making turn %d", t);
|
|
34 | 33 |
nav_map->update_state(t); |
35 |
Duration time = nav_map->get_time_remaining(); |
|
36 |
while(time.sec > 0) |
|
37 |
time = nav_map->get_time_remaining(); |
|
34 |
switch(t) |
|
35 |
{ |
|
36 |
case ISTRAIGHT: |
|
37 |
follow_line(); |
|
38 |
break; |
|
39 |
case ILEFT: |
|
40 |
turn_left(); |
|
41 |
follow_line(); |
|
42 |
break; |
|
43 |
case IRIGHT: |
|
44 |
turn_right(); |
|
45 |
follow_line(); |
|
46 |
break; |
|
47 |
case IUTURN: |
|
48 |
u_turn(); |
|
49 |
follow_line(); |
|
50 |
break; |
|
51 |
} |
|
38 | 52 |
} |
39 |
ROS_INFO("Path complete at state %d", (int)nav_map->get_state()); |
|
40 | 53 |
} |
41 | 54 |
|
42 | 55 |
int WH_Robot::exec_task() |
... | ... | |
141 | 154 |
spinOnce(); |
142 | 155 |
} |
143 | 156 |
|
157 |
follow_line(); |
|
158 |
|
|
144 | 159 |
while(ok()) |
145 | 160 |
{ |
146 | 161 |
ROS_INFO("WH_ROBOT: Running main loop"); |
scout/libscout/src/behaviors/WH_Robot.h | ||
---|---|---|
5 | 5 |
#define TASK_COMPLETED 0 |
6 | 6 |
#define TASK_FAILED -1 |
7 | 7 |
|
8 |
#include "line_follow.h" |
|
8 | 9 |
#include "../Behavior.h" |
9 | 10 |
#include "navigationMap.h" |
10 | 11 |
#include "../helper_classes/Order.h" |
11 | 12 |
#include <assert.h> |
12 | 13 |
#include <stdlib.h> |
13 | 14 |
|
14 |
class WH_Robot : Behavior{
|
|
15 |
class WH_Robot : line_follow{
|
|
15 | 16 |
std::string name; |
16 | 17 |
int id; |
17 | 18 |
|
... | ... | |
35 | 36 |
|
36 | 37 |
ros::Publisher robot_to_sched; |
37 | 38 |
ros::Subscriber sched_to_robot; |
38 |
|
|
39 | 39 |
}; |
40 | 40 |
|
41 | 41 |
#endif |
scout/libscout/src/behaviors/line_follow.cpp | ||
---|---|---|
36 | 36 |
{ |
37 | 37 |
double line_loc = linesensor->readline(); |
38 | 38 |
|
39 |
cout << "line_loc: " << line_loc << endl; |
|
40 |
|
|
41 | 39 |
motor_l = -MOTOR_BASE + SCALE * line_loc; |
42 | 40 |
motor_r = -MOTOR_BASE - SCALE * line_loc; |
43 | 41 |
|
44 | 42 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
45 | 43 |
} |
46 | 44 |
halt(); |
45 |
ROS_INFO("Intersection reached!"); |
|
47 | 46 |
} |
48 | 47 |
|
49 | 48 |
void line_follow::turn_left() |
... | ... | |
56 | 55 |
|
57 | 56 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
58 | 57 |
|
59 |
double line_loc = linesensor->readline(); |
|
60 |
cout << "line_loc: " << line_loc << endl; |
|
61 | 58 |
if(first) |
62 | 59 |
{ |
63 | 60 |
loop_rate->sleep(); |
... | ... | |
84 | 81 |
|
85 | 82 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
86 | 83 |
|
87 |
double line_loc = linesensor->readline(); |
|
88 |
cout << "line_loc: " << line_loc << endl; |
|
89 | 84 |
if(first) |
90 | 85 |
{ |
91 | 86 |
loop_rate->sleep(); |
... | ... | |
97 | 92 |
while(linesensor->readline()); |
98 | 93 |
} |
99 | 94 |
|
100 |
void line_follow::run()
|
|
95 |
void line_follow::u_turn()
|
|
101 | 96 |
{ |
97 |
turn_right(); |
|
102 | 98 |
follow_line(); |
103 | 99 |
turn_right(); |
100 |
} |
|
101 |
|
|
102 |
void line_follow::run() |
|
103 |
{ |
|
104 | 104 |
follow_line(); |
105 |
turn_left(); |
|
106 | 105 |
follow_line(); |
107 |
turn_right(); |
|
106 |
/* |
|
107 |
follow_line(); |
|
108 |
u_turn(); |
|
109 |
follow_line(); |
|
110 |
u_turn(); |
|
111 |
follow_line(); |
|
112 |
follow_line(); |
|
113 |
u_turn(); |
|
108 | 114 |
follow_line(); |
115 |
*/ |
|
109 | 116 |
} |
scout/libscout/src/behaviors/line_follow.h | ||
---|---|---|
31 | 31 |
#define MOTOR_BASE 50 |
32 | 32 |
#define SCALE 20 |
33 | 33 |
|
34 |
class line_follow : Behavior |
|
34 |
class line_follow : public Behavior
|
|
35 | 35 |
{ |
36 | 36 |
public: |
37 | 37 |
line_follow(std::string scoutname) : Behavior(scoutname, "line_follow") {}; |
... | ... | |
39 | 39 |
/** Actually executes the behavior. */ |
40 | 40 |
void run(); |
41 | 41 |
|
42 |
private:
|
|
42 |
protected:
|
|
43 | 43 |
void turn_left(); |
44 | 44 |
void turn_right(); |
45 |
void u_turn(); |
|
45 | 46 |
void follow_line(); |
46 | 47 |
void halt(); |
47 | 48 |
}; |
scout/libscout/src/behaviors/navigationMap.h | ||
---|---|---|
51 | 51 |
#define IRIGHT 2 |
52 | 52 |
#define IUTURN 3 |
53 | 53 |
|
54 |
#define START_STATE 1
|
|
54 |
#define START_STATE 3
|
|
55 | 55 |
|
56 | 56 |
#define DEADEND 255 |
57 | 57 |
#define ARRAY_SIZE 3 |
Also available in: Unified diff