Revision 6350051e
ID | 6350051e42d19a7a35b1e6bf62586ee87dba61bf |
Added a script to auto-generate BehaviorList files.
This includes some sub-changes: * Restructured behaviors/ folder into behaviors/, test_behaviors/ and limbo_behaviors/ * Added a new CMakeLists.txt that automatically finds behaviors in their folders * Removed the obsolete trafficNavigtaion.* * Added a new generator to GUI.py that looks in the behaviors folder for the behaviors automatically.
scout/libscout/CMakeLists.txt | ||
---|---|---|
30 | 30 |
#target_link_libraries(example ${PROJECT_NAME}) |
31 | 31 |
|
32 | 32 |
set(MAIN_FILES src/Sensors.cpp src/Behavior.cpp src/BehaviorList.cpp src/BehaviorProcess.cpp) |
33 |
set(BEHAVIOR_FILES src/behaviors/Odometry.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/behaviors/navigationMap.cpp src/behaviors/line_follow.cpp src/behaviors/wl_test.cpp src/behaviors/pause_scout.cpp) |
|
33 |
FILE(GLOB BEHAVIOR_FILES "${PROJECT_SOURCE_DIR}/src/behaviors/*.cpp") |
|
34 |
FILE(GLOB TEST_BEHAVIOR_FILES "${PROJECT_SOURCE_DIR}/src/test_behaviors/*.cpp") |
|
34 | 35 |
|
35 |
set(TEST_BEHAVIOR_FILES src/behaviors/Scheduler.cpp src/behaviors/WH_Robot.cpp src/behaviors/maze_solve.cpp) |
|
36 | 36 |
set(HELPER_FILES src/helper_classes/Order.cpp src/helper_classes/PQWrapper.cpp) |
37 | 37 |
set(CONTROL_CLASSES src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/WirelessSender.cpp src/WirelessReceiver.cpp src/EncodersControl.cpp src/LinesensorControl.cpp) |
38 | 38 |
|
scout/libscout/generate_behavior_lists.py | ||
---|---|---|
1 |
#!/usr/bin/python |
|
2 |
|
|
3 |
# Usage: |
|
4 |
# |
|
5 |
# python generate_behavior_lists.py |
|
6 |
# |
|
7 |
# Parses to use only the final folder and file name for each source file. |
|
8 |
|
|
9 |
import sys |
|
10 |
import os |
|
11 |
|
|
12 |
AUTOGEN_HEADER = '''/** |
|
13 |
****************************************************************************** |
|
14 |
* WARNING: THIS IS AN AUTOGENERATED FILE! |
|
15 |
* |
|
16 |
* Editing this file is USELESS. It will be overwritten during the build. |
|
17 |
* To properly edit this file, edit its corresponding *.template.* file, which |
|
18 |
* a script run by CMake uses to generate this file. |
|
19 |
* |
|
20 |
* This file was generated via rules in src/generate_behavior_lists.py. |
|
21 |
* |
|
22 |
* Please see that file for a description of syntax and procedure for |
|
23 |
* creating auto-generated files. |
|
24 |
* |
|
25 |
* THIS WARNING IS LONG TO MAKE SURE IT GETS YOUR ATTENTION! |
|
26 |
* THANK YOU. |
|
27 |
***************************************************************************** |
|
28 |
*/ |
|
29 |
|
|
30 |
''' |
|
31 |
|
|
32 |
# These are the codes this script will replace in template files. |
|
33 |
FULL_PATH_CODE = 'AUTOGEN_BEHAVIOR_FULL_PATH' |
|
34 |
LAST_PATH_CODE = 'AUTOGEN_BEHAVIOR_LAST_PATH' |
|
35 |
BASE_NAME_CODE = 'AUTOGEN_BEHAVIOR_BASE_NAME' |
|
36 |
|
|
37 |
# Different ways to refer to files in behaviors/ and test_behaviors/. |
|
38 |
full_paths = [] |
|
39 |
last_paths = [] |
|
40 |
base_names = [] |
|
41 |
|
|
42 |
# Mapping of generated files to their templates |
|
43 |
R_FILES = {'src/BehaviorList.cpp' : 'src/BehaviorList.template.cpp', |
|
44 |
'src/BehaviorList.h' : 'src/BehaviorList.template.h'} |
|
45 |
|
|
46 |
# Parse the src/behaviors and src/test_behaviors folders |
|
47 |
cur_dir = os.getcwd() |
|
48 |
for path, dirname, fnames in os.walk('src'): |
|
49 |
if path == 'src/behaviors' or path == 'src/test_behaviors': |
|
50 |
path_parts = path.split('/') |
|
51 |
|
|
52 |
for f in sorted(fnames): |
|
53 |
# The pause_scout behavior needs to go first! |
|
54 |
if f.endswith('.h') and 'pause_scout' in f: |
|
55 |
full_paths.insert(0, os.path.join(cur_dir, path, f)) |
|
56 |
last_paths.insert(0, os.path.join(path_parts[-1], f)) |
|
57 |
base_names.insert(0, f.split('.')[0]) |
|
58 |
# Everything else goes in alphabetical order |
|
59 |
elif f.endswith('.h'): |
|
60 |
full_paths.append(os.path.join(cur_dir, path, f)) |
|
61 |
last_paths.append(os.path.join(path_parts[-1], f)) |
|
62 |
base_names.append(f.split('.')[0]) |
|
63 |
|
|
64 |
# Now, a dictionary of codes to replace -> things to replace them with. |
|
65 |
replacements = {FULL_PATH_CODE : full_paths, |
|
66 |
LAST_PATH_CODE : last_paths, |
|
67 |
BASE_NAME_CODE : base_names} |
|
68 |
|
|
69 |
# Write all the replacements in R_FILES |
|
70 |
for outfile_name in R_FILES.keys(): |
|
71 |
with open(outfile_name, 'w') as outfile: |
|
72 |
with open(R_FILES[outfile_name], 'r') as infile: |
|
73 |
|
|
74 |
# Write an obvious header so no one will edit our nicely |
|
75 |
# generated file |
|
76 |
outfile.write(AUTOGEN_HEADER) |
|
77 |
|
|
78 |
for line in infile.readlines(): |
|
79 |
# If the line needs replacing, don't write the orgininal |
|
80 |
# to the output file; rather, write many versions of the |
|
81 |
# line replaced with each replacement value. |
|
82 |
replaced = False |
|
83 |
for r_key in replacements: |
|
84 |
if r_key in line: |
|
85 |
replaced = True |
|
86 |
for b in replacements[r_key]: |
|
87 |
outfile.write(line.replace(r_key, b)) |
|
88 |
# If it doesn't need replacing, just write the original |
|
89 |
if not replaced: |
|
90 |
outfile.write(line) |
scout/libscout/src/BehaviorList.cpp | ||
---|---|---|
1 |
#include "BehaviorList.h" |
|
1 |
/** |
|
2 |
****************************************************************************** |
|
3 |
* WARNING: THIS IS AN AUTOGENERATED FILE! |
|
4 |
* |
|
5 |
* Editing this file is USELESS. It will be overwritten during the build. |
|
6 |
* To properly edit this file, edit its corresponding *.template.* file, which |
|
7 |
* a script run by CMake uses to generate this file. |
|
8 |
* |
|
9 |
* This file was generated via rules in src/generate_behavior_lists.py. |
|
10 |
* |
|
11 |
* Please see that file for a description of syntax and procedure for |
|
12 |
* creating auto-generated files. |
|
13 |
* |
|
14 |
* THIS WARNING IS LONG TO MAKE SURE IT GETS YOUR ATTENTION! |
|
15 |
* THANK YOU. |
|
16 |
***************************************************************************** |
|
17 |
*/ |
|
2 | 18 |
|
19 |
#include "BehaviorList.h" |
|
3 | 20 |
|
4 | 21 |
BehaviorList::BehaviorList() |
5 | 22 |
{ |
6 | 23 |
behavior_list.push_back(behavior<pause_scout>); |
7 |
behavior_list.push_back(behavior<draw_cw_circle>); |
|
8 |
behavior_list.push_back(behavior<draw_ccw_circle>); |
|
9 | 24 |
behavior_list.push_back(behavior<Odometry>); |
25 |
behavior_list.push_back(behavior<draw_ccw_circle>); |
|
26 |
behavior_list.push_back(behavior<draw_cw_circle>); |
|
27 |
behavior_list.push_back(behavior<line_follow>); |
|
10 | 28 |
behavior_list.push_back(behavior<navigationMap>); |
29 |
behavior_list.push_back(behavior<wl_test>); |
|
11 | 30 |
behavior_list.push_back(behavior<Scheduler>); |
12 | 31 |
behavior_list.push_back(behavior<WH_Robot>); |
13 |
behavior_list.push_back(behavior<line_follow>); |
|
14 |
behavior_list.push_back(behavior<wl_test>); |
|
15 | 32 |
behavior_list.push_back(behavior<maze_solve>); |
16 | 33 |
return; |
17 | 34 |
} |
scout/libscout/src/BehaviorList.h | ||
---|---|---|
1 |
/** |
|
2 |
****************************************************************************** |
|
3 |
* WARNING: THIS IS AN AUTOGENERATED FILE! |
|
4 |
* |
|
5 |
* Editing this file is USELESS. It will be overwritten during the build. |
|
6 |
* To properly edit this file, edit its corresponding *.template.* file, which |
|
7 |
* a script run by CMake uses to generate this file. |
|
8 |
* |
|
9 |
* This file was generated via rules in src/generate_behavior_lists.py. |
|
10 |
* |
|
11 |
* Please see that file for a description of syntax and procedure for |
|
12 |
* creating auto-generated files. |
|
13 |
* |
|
14 |
* THIS WARNING IS LONG TO MAKE SURE IT GETS YOUR ATTENTION! |
|
15 |
* THANK YOU. |
|
16 |
***************************************************************************** |
|
17 |
*/ |
|
18 |
|
|
1 | 19 |
#ifndef _BEHAVIOR_LIST_H_ |
2 | 20 |
#define _BEHAVIOR_LIST_H_ |
3 | 21 |
|
4 | 22 |
#include "Behavior.h" |
5 |
#include "behaviors/line_follow.h" |
|
6 |
#include "behaviors/draw_cw_circle.h" |
|
7 |
#include "behaviors/draw_ccw_circle.h" |
|
23 |
#include "Sensors.h" |
|
24 |
#include "behaviors/pause_scout.h" |
|
8 | 25 |
#include "behaviors/Odometry.h" |
26 |
#include "behaviors/draw_ccw_circle.h" |
|
27 |
#include "behaviors/draw_cw_circle.h" |
|
28 |
#include "behaviors/line_follow.h" |
|
9 | 29 |
#include "behaviors/navigationMap.h" |
10 |
#include "behaviors/Scheduler.h" |
|
11 |
#include "behaviors/WH_Robot.h" |
|
12 | 30 |
#include "behaviors/wl_test.h" |
13 |
#include "behaviors/pause_scout.h"
|
|
14 |
#include "behaviors/maze_solve.h"
|
|
15 |
#include "Sensors.h"
|
|
31 |
#include "test_behaviors/Scheduler.h"
|
|
32 |
#include "test_behaviors/WH_Robot.h"
|
|
33 |
#include "test_behaviors/maze_solve.h"
|
|
16 | 34 |
|
17 | 35 |
template<typename T> Behavior* behavior(std::string scoutname, Sensors* sensors){ return (Behavior*)new T(scoutname, sensors); } |
18 | 36 |
|
scout/libscout/src/behaviors/Scheduler.cpp | ||
---|---|---|
1 |
#include "Scheduler.h" |
|
2 |
#include "../helper_classes/Order.h" |
|
3 |
|
|
4 |
using namespace std; |
|
5 |
|
|
6 |
/** @Brief: Initialize data structures for the scheduler. */ |
|
7 |
Scheduler::Scheduler(std::string scoutname, Sensors* sensors): |
|
8 |
Behavior(scoutname, "Scheduler", sensors) |
|
9 |
{ |
|
10 |
unassignedOrders = new PQWrapper(NUM_TASKS); |
|
11 |
|
|
12 |
create_orders(); |
|
13 |
} |
|
14 |
|
|
15 |
/** @Brief: Free allocatetd memory. */ |
|
16 |
Scheduler::~Scheduler() |
|
17 |
{ |
|
18 |
delete unassignedOrders; |
|
19 |
} |
|
20 |
|
|
21 |
/** @Brief: Add robot to the waiting queue. |
|
22 |
* A robot calls this function with itself |
|
23 |
* and gets pushed on a list of waiting robots. |
|
24 |
* When a task is availaible the scheduler, removes |
|
25 |
* the robot of the waiting queue, and gives it a |
|
26 |
* task. |
|
27 |
*/ |
|
28 |
void Scheduler::get_task(int robot) |
|
29 |
{ |
|
30 |
Robot my_robot = robots[robot-1]; |
|
31 |
if(my_robot.sched_status != WAITING_ROBOT) |
|
32 |
{ |
|
33 |
waitingRobots.push(my_robot); |
|
34 |
my_robot.sched_status = WAITING_ROBOT; |
|
35 |
} |
|
36 |
} |
|
37 |
|
|
38 |
/** @Brief: Statically add orders to the Priority Queue Wrapper.*/ |
|
39 |
void Scheduler::create_orders() |
|
40 |
{ |
|
41 |
Path p; |
|
42 |
p.len = 0; |
|
43 |
p.path = NULL; |
|
44 |
Time t = ros::Time::now(); |
|
45 |
Duration end(0); |
|
46 |
Duration five(5); |
|
47 |
Duration three(3); |
|
48 |
Duration ten(10); |
|
49 |
Order a(1,0,13,t+ten,p,end); |
|
50 |
Order b(2,0,14,t+five,p,end); |
|
51 |
Order c(3,0,15,t+three,p,end); |
|
52 |
Order d(4,0,16,t,p,end); |
|
53 |
|
|
54 |
unassignedOrders->insert(a); |
|
55 |
unassignedOrders->insert(b); |
|
56 |
unassignedOrders->insert(c); |
|
57 |
unassignedOrders->insert(d); |
|
58 |
} |
|
59 |
|
|
60 |
/** @Brief: This is a confirmation that the task is complete. |
|
61 |
This function removes the order from assignedOrders. */ |
|
62 |
void Scheduler::task_complete(Order o) |
|
63 |
{ |
|
64 |
ROS_INFO("Scheduler: Task id %d was completed", o.getid()); |
|
65 |
for (unsigned int i=0; i<assignedOrders.size(); i++) |
|
66 |
{ |
|
67 |
if (assignedOrders[i].getid()==o.getid()) |
|
68 |
{ |
|
69 |
assignedOrders.erase(assignedOrders.begin()+i); |
|
70 |
} |
|
71 |
} |
|
72 |
} |
|
73 |
|
|
74 |
/** @Brief: This is a confirmation that the task failed. |
|
75 |
This function places the order back on the PQWrapper. */ |
|
76 |
void Scheduler::task_failed(Order o) |
|
77 |
{ |
|
78 |
ROS_INFO("Scheduler: Task id %d was failed", o.getid()); |
|
79 |
task_complete(o); |
|
80 |
unassignedOrders->insert(o); |
|
81 |
} |
|
82 |
|
|
83 |
/** @Brief: Do a waiting dance. */ |
|
84 |
void Scheduler::waiting_dance() |
|
85 |
{ |
|
86 |
//ROS_INFO("Scheduler: TEEHEE i do a dance!"); |
|
87 |
} |
|
88 |
|
|
89 |
/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */ |
|
90 |
Order Scheduler::get_next_item() |
|
91 |
{ |
|
92 |
Time t = ros::TIME_MAX; |
|
93 |
double time = t.toSec(); |
|
94 |
|
|
95 |
Order* best; |
|
96 |
int best_index; |
|
97 |
for(unsigned int i=0; i<unassignedOrders->arraySize(); i++) |
|
98 |
{ |
|
99 |
Order order = unassignedOrders->peek(i); |
|
100 |
|
|
101 |
Time end_time = order.get_start_time() - order.get_est_time(); |
|
102 |
|
|
103 |
if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function |
|
104 |
{ |
|
105 |
best = ℴ |
|
106 |
best_index = i; |
|
107 |
time = end_time.toSec() + MAX_WAIT_TIME; |
|
108 |
} |
|
109 |
} |
|
110 |
|
|
111 |
Order ret; |
|
112 |
ret = unassignedOrders->remove(best_index); |
|
113 |
assignedOrders.push_back(ret); |
|
114 |
return ret; |
|
115 |
} |
|
116 |
|
|
117 |
void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg) |
|
118 |
{ |
|
119 |
if(msg->data.compare(0, 6, "FAILED") == 0) |
|
120 |
{ |
|
121 |
int order_id = atoi(msg->data.substr(7).c_str()); |
|
122 |
for(unsigned int i=0; i<assignedOrders.size(); i++) |
|
123 |
{ |
|
124 |
if(assignedOrders[i].getid() == order_id) |
|
125 |
{ |
|
126 |
task_failed(assignedOrders[i]); |
|
127 |
break; |
|
128 |
} |
|
129 |
} |
|
130 |
} |
|
131 |
else if(msg->data.compare(0, 7, "SUCCESS") == 0) |
|
132 |
{ |
|
133 |
int order_id = atoi(msg->data.substr(8).c_str()); |
|
134 |
for(unsigned int i=0; i<assignedOrders.size(); i++) |
|
135 |
{ |
|
136 |
if(assignedOrders[i].getid() == order_id) |
|
137 |
{ |
|
138 |
task_complete(assignedOrders[i]); |
|
139 |
break; |
|
140 |
} |
|
141 |
} |
|
142 |
} |
|
143 |
else if(msg->data.compare(0, 8, "GET_TASK") == 0) |
|
144 |
{ |
|
145 |
ROS_INFO("SCHEDULER: got get task message"); |
|
146 |
int robot = atoi(msg->data.substr(9).c_str()); |
|
147 |
get_task(robot); |
|
148 |
} |
|
149 |
else if(msg->data.compare(0, 8, "REGISTER") == 0) |
|
150 |
{ |
|
151 |
string robot_name = msg->data.substr(9); |
|
152 |
for(unsigned int i=0; i<robots.size(); i++) |
|
153 |
{ |
|
154 |
if(robots[i].name.compare(robot_name) == 0) |
|
155 |
{ |
|
156 |
return; |
|
157 |
} |
|
158 |
} |
|
159 |
|
|
160 |
int id = robots.size() +1; |
|
161 |
Robot new_robot; |
|
162 |
new_robot.name = robot_name; |
|
163 |
new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000); |
|
164 |
new_robot.sched_status = NEW_ROBOT; |
|
165 |
robots.push_back(new_robot); |
|
166 |
|
|
167 |
std_msgs::String msg; |
|
168 |
std::stringstream ss; |
|
169 |
ss<<"REG_SUCCESS "<<id; |
|
170 |
msg.data = ss.str(); |
|
171 |
new_robot.topic.publish(msg); |
|
172 |
spinOnce(); |
|
173 |
|
|
174 |
ROS_INFO("Registration a success"); |
|
175 |
|
|
176 |
assert(robots.size() > 0); |
|
177 |
} |
|
178 |
else |
|
179 |
{ |
|
180 |
ROS_INFO("I got a bad message: %s", msg->data.c_str()); |
|
181 |
} |
|
182 |
|
|
183 |
return; |
|
184 |
} |
|
185 |
|
|
186 |
/** @Brief: Continuously checks for waiting robots. If no robots are waiting, |
|
187 |
this function calls the waiting_dance() function. */ |
|
188 |
void Scheduler::run() |
|
189 |
{ |
|
190 |
robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this); |
|
191 |
ROS_INFO("I am a scheduler!. Now waiting for robots"); |
|
192 |
while(robots.size() < 1 && ok()) |
|
193 |
{ |
|
194 |
spinOnce(); |
|
195 |
} |
|
196 |
ROS_INFO("SCHEDULER: main loop"); |
|
197 |
while (ok()) |
|
198 |
{ |
|
199 |
ROS_INFO("Scheduler running"); |
|
200 |
spinOnce(); |
|
201 |
while((waitingRobots.empty() || unassignedOrders->arraySize()==0) && ok()) |
|
202 |
{ |
|
203 |
waiting_dance(); |
|
204 |
spinOnce(); |
|
205 |
} |
|
206 |
|
|
207 |
ROS_INFO("SCHEDULER: Setting task"); |
|
208 |
Order next = get_next_item(); |
|
209 |
Robot r = waitingRobots.front(); |
|
210 |
|
|
211 |
ROS_INFO("SCHEDULER: Setting task %d to robot %s", next.getid(), r.name.c_str()); |
|
212 |
std_msgs::String msg; |
|
213 |
std::stringstream ss; |
|
214 |
ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest(); |
|
215 |
std::cout<<"msg: "<<ss.str()<<endl; |
|
216 |
msg.data = ss.str(); |
|
217 |
r.topic.publish(msg); |
|
218 |
|
|
219 |
spinOnce(); |
|
220 |
|
|
221 |
waitingRobots.front().sched_status = ORDERED_ROBOT; |
|
222 |
waitingRobots.pop(); |
|
223 |
} |
|
224 |
} |
|
225 |
|
|
226 |
|
|
227 |
|
|
228 |
|
scout/libscout/src/behaviors/Scheduler.h | ||
---|---|---|
1 |
#ifndef _SCHEDULER_ |
|
2 |
#define _SCHEDULER_ |
|
3 |
|
|
4 |
#include "../helper_classes/PQWrapper.h" |
|
5 |
#include "../helper_classes/Order.h" |
|
6 |
#include "../Behavior.h" |
|
7 |
|
|
8 |
#define NUM_TASKS 5 |
|
9 |
#define WAITING_ROBOT 1 |
|
10 |
#define NEW_ROBOT 2 |
|
11 |
#define ORDERED_ROBOT 3 |
|
12 |
|
|
13 |
typedef struct{ |
|
14 |
std::string name; |
|
15 |
ros::Publisher topic; |
|
16 |
int sched_status; |
|
17 |
} Robot; |
|
18 |
|
|
19 |
class Scheduler : Behavior { |
|
20 |
std::vector<Robot> robots; |
|
21 |
std::queue<Robot> waitingRobots; |
|
22 |
|
|
23 |
PQWrapper* unassignedOrders; |
|
24 |
std::vector<Order> assignedOrders; |
|
25 |
|
|
26 |
void create_orders(); |
|
27 |
|
|
28 |
void waiting_dance(); |
|
29 |
|
|
30 |
void msg_callback(const std_msgs::String::ConstPtr& msg); |
|
31 |
|
|
32 |
public: |
|
33 |
Scheduler(std::string scoutname, Sensors* sensors); |
|
34 |
~Scheduler(); |
|
35 |
|
|
36 |
void get_task(int robot); |
|
37 |
|
|
38 |
void task_complete(Order o); |
|
39 |
void task_failed(Order o); |
|
40 |
|
|
41 |
|
|
42 |
Order get_next_item(); |
|
43 |
|
|
44 |
void run(); |
|
45 |
|
|
46 |
ros::Subscriber robot_to_sched; |
|
47 |
|
|
48 |
}; |
|
49 |
#endif |
scout/libscout/src/behaviors/WH_Robot.cpp | ||
---|---|---|
1 |
#include "WH_Robot.h" |
|
2 |
#include "../helper_classes/Order.h" |
|
3 |
|
|
4 |
/** @Brief: warehouse robot constructor **/ |
|
5 |
WH_Robot::WH_Robot(std::string scoutname, Sensors* sensors): |
|
6 |
line_follow(scoutname, sensors) |
|
7 |
{ |
|
8 |
nav_map = new navigationMap(scoutname, sensors); |
|
9 |
curr_task = DEFAULT_TASK; |
|
10 |
name = scoutname; |
|
11 |
reg_failed = 1; |
|
12 |
srand(0xDEADBEEF); |
|
13 |
} |
|
14 |
|
|
15 |
WH_Robot::~WH_Robot() |
|
16 |
{ |
|
17 |
delete(nav_map); |
|
18 |
} |
|
19 |
|
|
20 |
Duration WH_Robot::get_worst_case_time(State start_state, State target_state) |
|
21 |
{ |
|
22 |
return nav_map->get_worst_case_time(start_state, target_state); |
|
23 |
} |
|
24 |
|
|
25 |
/** @brief Drives from state 2 to the robot home base and goes to sleep for |
|
26 |
* x seconds |
|
27 |
*/ |
|
28 |
void WH_Robot::go_home(int x) |
|
29 |
{ |
|
30 |
unsigned int curr_state = nav_map->get_state(); |
|
31 |
if(curr_state != 2) |
|
32 |
{ |
|
33 |
ROS_WARN("Tried to go home from incorrect state %d", curr_state); |
|
34 |
return; |
|
35 |
} |
|
36 |
turn_straight(); |
|
37 |
follow_line(); |
|
38 |
turn_left(); |
|
39 |
follow_line(); |
|
40 |
Duration sleep_time(x); |
|
41 |
sleep_time.sleep(); |
|
42 |
return; |
|
43 |
} |
|
44 |
|
|
45 |
void WH_Robot::leave_home() |
|
46 |
{ |
|
47 |
unsigned int curr_state = nav_map->get_state(); |
|
48 |
if(curr_state != 2) |
|
49 |
{ |
|
50 |
ROS_WARN("Tried to go home from incorrect state %d", curr_state); |
|
51 |
return; |
|
52 |
} |
|
53 |
turn_straight(); |
|
54 |
follow_line(); |
|
55 |
nav_map->update_state(ISTRAIGHT); |
|
56 |
turn_left(); |
|
57 |
follow_line(); |
|
58 |
} |
|
59 |
|
|
60 |
/** @brief Based on the given path, make |
|
61 |
* the series of turns to follow this path, |
|
62 |
* updating the current state in the navigation map as we do so |
|
63 |
* @param the Path to follow |
|
64 |
*/ |
|
65 |
void WH_Robot::follow_path(Path path_to_follow){ |
|
66 |
for(int i=0; i<path_to_follow.len; i++) |
|
67 |
{ |
|
68 |
Turn t = path_to_follow.path[i]; |
|
69 |
unsigned int curr_state = nav_map->get_state(); |
|
70 |
|
|
71 |
nav_map->update_state(t); |
|
72 |
switch(t) |
|
73 |
{ |
|
74 |
case ISTRAIGHT: |
|
75 |
turn_straight(); |
|
76 |
follow_line(); |
|
77 |
if(curr_state == 2) |
|
78 |
{ |
|
79 |
turn_straight(); |
|
80 |
follow_line(); |
|
81 |
turn_straight(); |
|
82 |
follow_line(); |
|
83 |
} |
|
84 |
break; |
|
85 |
case ILEFT: |
|
86 |
turn_left(); |
|
87 |
follow_line(); |
|
88 |
break; |
|
89 |
case IRIGHT: |
|
90 |
if(9 <= curr_state && curr_state <= 12) |
|
91 |
{ |
|
92 |
turn_straight(); |
|
93 |
follow_line(); |
|
94 |
} |
|
95 |
turn_right(); |
|
96 |
follow_line(); |
|
97 |
if(curr_state <= 4) |
|
98 |
{ |
|
99 |
turn_straight(); |
|
100 |
follow_line(); |
|
101 |
} |
|
102 |
break; |
|
103 |
case IUTURN: |
|
104 |
u_turn(); |
|
105 |
follow_line(); |
|
106 |
break; |
|
107 |
case ISPOTTURN: |
|
108 |
spot_turn(); |
|
109 |
follow_line(); |
|
110 |
break; |
|
111 |
} |
|
112 |
} |
|
113 |
} |
|
114 |
|
|
115 |
int WH_Robot::exec_task() |
|
116 |
{ |
|
117 |
assert(curr_task != DEFAULT_TASK); |
|
118 |
|
|
119 |
// remember where you started doing this task so we know where to return |
|
120 |
// State home_state = nav_map->get_state(); |
|
121 |
// For now, use state 2. |
|
122 |
Path new_path = nav_map->shortest_path(curr_task->get_dest()); |
|
123 |
curr_task->set_path(new_path); |
|
124 |
|
|
125 |
follow_path(new_path); |
|
126 |
|
|
127 |
//TODO: forklift yaaaay |
|
128 |
|
|
129 |
int did_task_complete; |
|
130 |
|
|
131 |
int error = rand() % 12; |
|
132 |
if(error < 9) //Fail with 1/4 probability |
|
133 |
{ |
|
134 |
ROS_INFO("WH_robot: TASK COMPLETE!"); |
|
135 |
did_task_complete = TASK_COMPLETED; |
|
136 |
} |
|
137 |
else |
|
138 |
{ |
|
139 |
ROS_INFO("WH_robot: TASK FAILED!"); |
|
140 |
did_task_complete = TASK_FAILED; |
|
141 |
} |
|
142 |
|
|
143 |
// For now use state 2 as home state |
|
144 |
// Path return_path = nav_map->shortest_path(home_state); |
|
145 |
Path return_path = nav_map->shortest_path(2); |
|
146 |
curr_task->set_path(return_path); |
|
147 |
|
|
148 |
follow_path(return_path); |
|
149 |
|
|
150 |
return did_task_complete; |
|
151 |
} |
|
152 |
|
|
153 |
void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg) |
|
154 |
{ |
|
155 |
if(msg->data.compare(0, 11, "REG_SUCCESS") == 0) |
|
156 |
{ |
|
157 |
id = atoi(msg->data.substr(12).c_str()); |
|
158 |
reg_failed = 0; |
|
159 |
} |
|
160 |
else if(msg->data.compare(0, 8, "SET_TASK") == 0) |
|
161 |
{ |
|
162 |
char* string = (char*)msg->data.c_str(); |
|
163 |
char* data; |
|
164 |
data = strtok(string, " "); |
|
165 |
|
|
166 |
data = strtok(NULL, " "); |
|
167 |
int order_id = atoi(data); |
|
168 |
|
|
169 |
data = strtok(NULL, " "); |
|
170 |
Address source = atoi(data); |
|
171 |
|
|
172 |
data = strtok(NULL, " "); |
|
173 |
Address dest = atoi(data); |
|
174 |
|
|
175 |
Time start_time = ros::Time::now(); |
|
176 |
Path path; |
|
177 |
path.len = 0; |
|
178 |
path.path = NULL; |
|
179 |
Duration est_time(0); |
|
180 |
|
|
181 |
curr_task = new Order(order_id, source, dest, start_time, path, est_time); |
|
182 |
} |
|
183 |
else |
|
184 |
{ |
|
185 |
ROS_INFO("I got a bad message: %s", msg->data.c_str()); |
|
186 |
} |
|
187 |
} |
|
188 |
|
|
189 |
void WH_Robot::run () |
|
190 |
{ |
|
191 |
ROS_INFO("A WH_Robot has been created"); |
|
192 |
|
|
193 |
robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000); |
|
194 |
sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this); |
|
195 |
|
|
196 |
ROS_INFO("I am a robot. Registering with scheduler..."); |
|
197 |
while(reg_failed && ok()) |
|
198 |
{ |
|
199 |
std_msgs::String msg; |
|
200 |
std::stringstream ss; |
|
201 |
ss << "REGISTER "<<name; |
|
202 |
msg.data = ss.str(); |
|
203 |
robot_to_sched.publish(msg); |
|
204 |
|
|
205 |
spinOnce(); |
|
206 |
} |
|
207 |
|
|
208 |
follow_line(); |
|
209 |
|
|
210 |
while(ok()) |
|
211 |
{ |
|
212 |
ROS_INFO("WH_ROBOT: Running main loop"); |
|
213 |
|
|
214 |
|
|
215 |
std_msgs::String msg; |
|
216 |
std::stringstream ss; |
|
217 |
ss << "GET_TASK "<<id; |
|
218 |
msg.data = ss.str(); |
|
219 |
robot_to_sched.publish(msg); |
|
220 |
|
|
221 |
while(curr_task == DEFAULT_TASK && ok()){ |
|
222 |
spinOnce(); |
|
223 |
} |
|
224 |
|
|
225 |
int error = exec_task(); |
|
226 |
|
|
227 |
/** Go to robot home base */ |
|
228 |
go_home(2); |
|
229 |
leave_home(); |
|
230 |
|
|
231 |
if(error == TASK_COMPLETED) |
|
232 |
{ |
|
233 |
std_msgs::String msg; |
|
234 |
std::stringstream ss; |
|
235 |
ss << "SUCCESS "<<curr_task->getid(); |
|
236 |
msg.data = ss.str(); |
|
237 |
robot_to_sched.publish(msg); |
|
238 |
} |
|
239 |
else //error == TASK_FAILED |
|
240 |
{ |
|
241 |
std_msgs::String msg; |
|
242 |
std::stringstream ss; |
|
243 |
ss << "FAILED "<<curr_task->getid(); |
|
244 |
msg.data = ss.str(); |
|
245 |
robot_to_sched.publish(msg); |
|
246 |
} |
|
247 |
delete curr_task; |
|
248 |
curr_task = DEFAULT_TASK; |
|
249 |
|
|
250 |
} |
|
251 |
} |
|
252 |
|
|
253 |
void WH_Robot::set_task(Order order) |
|
254 |
{ |
|
255 |
curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), |
|
256 |
order.get_start_time(), order.get_path(), order.get_est_time()); |
|
257 |
return; |
|
258 |
} |
|
259 |
|
scout/libscout/src/behaviors/WH_Robot.h | ||
---|---|---|
1 |
#ifndef _WH_ROBOT_ |
|
2 |
#define _WH_ROBOT_ |
|
3 |
|
|
4 |
#define DEFAULT_TASK NULL |
|
5 |
#define TASK_COMPLETED 0 |
|
6 |
#define TASK_FAILED -1 |
|
7 |
|
|
8 |
#include "line_follow.h" |
|
9 |
#include "../Behavior.h" |
|
10 |
#include "navigationMap.h" |
|
11 |
#include "../helper_classes/Order.h" |
|
12 |
#include <assert.h> |
|
13 |
#include <stdlib.h> |
|
14 |
|
|
15 |
class WH_Robot : line_follow{ |
|
16 |
std::string name; |
|
17 |
int id; |
|
18 |
|
|
19 |
int reg_failed; |
|
20 |
|
|
21 |
Order* curr_task; |
|
22 |
navigationMap* nav_map; |
|
23 |
|
|
24 |
Duration get_worst_case_time(State start_state, State target_state); |
|
25 |
int exec_task(); |
|
26 |
|
|
27 |
void robot_callback(const std_msgs::String::ConstPtr& msg); |
|
28 |
|
|
29 |
void go_home(int x); |
|
30 |
void leave_home(); |
|
31 |
|
|
32 |
public: |
|
33 |
WH_Robot(std::string scoutname, Sensors* sensors); |
|
34 |
~WH_Robot(); |
|
35 |
void run(); |
|
36 |
|
|
37 |
void set_task(Order order); |
|
38 |
void follow_path(Path path_to_follow); |
|
39 |
|
|
40 |
ros::Publisher robot_to_sched; |
|
41 |
ros::Subscriber sched_to_robot; |
|
42 |
}; |
|
43 |
|
|
44 |
#endif |
scout/libscout/src/behaviors/maze_solve.cpp | ||
---|---|---|
1 |
/** |
|
2 |
* Copyright (c) 2011 Colony Project |
|
3 |
* |
|
4 |
* Permission is hereby granted, free of charge, to any person |
|
5 |
* obtaining a copy of this software and associated documentation |
|
6 |
* files (the "Software"), to deal in the Software without |
|
7 |
* restriction, including without limitation the rights to use, |
|
8 |
* copy, modify, merge, publish, distribute, sublicense, and/or sell |
|
9 |
* copies of the Software, and to permit persons to whom the |
|
10 |
* Software is furnished to do so, subject to the following |
|
11 |
* conditions: |
|
12 |
* |
|
13 |
* The above copyright notice and this permission notice shall be |
|
14 |
* included in all copies or substantial portions of the Software. |
|
15 |
* |
|
16 |
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
|
17 |
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES |
|
18 |
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
|
19 |
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT |
|
20 |
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, |
|
21 |
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING |
|
22 |
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
|
23 |
* OTHER DEALINGS IN THE SOFTWARE. |
|
24 |
*/ |
|
25 |
|
|
26 |
#include "maze_solve.h" |
|
27 |
|
|
28 |
using namespace std; |
|
29 |
|
|
30 |
|
|
31 |
// want to have a minimal working thing, use a big enough |
|
32 |
// static array and start in the middle |
|
33 |
// we assume we are facing right, that affects where we store |
|
34 |
// wall information |
|
35 |
// -1 for wall, 0 for unseen, 1 for traveled, 2 for critical |
|
36 |
#define WALL -1 |
|
37 |
#define UNSEEN 0 |
|
38 |
#define SEEN 1 |
|
39 |
#define CRITICAL 2 |
|
40 |
// facings |
|
41 |
#define UP 0 |
|
42 |
#define RIGHT 1 |
|
43 |
#define DOWN 2 |
|
44 |
#define LEFT 3 |
|
45 |
|
|
46 |
Duration sonar_update_time(1.5); |
|
47 |
|
|
48 |
void maze_solve::run(){ |
|
49 |
|
|
50 |
// TODO:first initialize map to all 0's |
|
51 |
ROS_INFO("Starting to solve the maze"); |
|
52 |
// Go up to the first line. |
|
53 |
follow_line(); |
|
54 |
// Turn the sonar on. |
|
55 |
sonar->set_on(); |
|
56 |
sonar->set_range(0, 23); |
|
57 |
|
|
58 |
// Wait for the sonar to initialize. |
|
59 |
while(!look_around(25, 25, RIGHT) && ok()) |
|
60 |
{ |
|
61 |
spinOnce(); |
|
62 |
} |
|
63 |
|
|
64 |
// Solve the maze |
|
65 |
bool finished = solve(25,25, RIGHT); |
|
66 |
|
|
67 |
// Check and report final condition. |
|
68 |
if (finished) |
|
69 |
ROS_INFO("YAY! I have solved the maze"); |
|
70 |
else |
|
71 |
ROS_INFO("NO! The maze is unsolvable"); |
|
72 |
} |
|
73 |
|
|
74 |
bool maze_solve::solve(int row, int col, int dir) |
|
75 |
{ |
|
76 |
int initial_dir = dir; |
|
77 |
|
|
78 |
ROS_INFO("I am at direction %d", dir); |
|
79 |
|
|
80 |
// use backtracking to solve the maze |
|
81 |
if (at_destination()) |
|
82 |
return true; |
|
83 |
|
|
84 |
// Wait for sonar to update. |
|
85 |
sonar_update_time.sleep(); |
|
86 |
|
|
87 |
// this function should fill the adjacent cells around me with |
|
88 |
// wall's or paths |
|
89 |
while(!look_around(row, col, dir) && ok()) |
|
90 |
{ |
|
91 |
spinOnce(); |
|
92 |
} |
|
93 |
|
|
94 |
/* try go up */ |
|
95 |
if (map[row-1][col] != WALL && initial_dir != UP) |
|
96 |
{ |
|
97 |
ROS_INFO("GOING UP!"); |
|
98 |
// Turn up. |
|
99 |
turn_from_to(dir, UP); |
|
100 |
follow_line(); |
|
101 |
// Solve recursively. |
|
102 |
bool solved = solve(row-1, col, DOWN); |
|
103 |
if (solved) |
|
104 |
{ |
|
105 |
return solved; |
|
106 |
} |
|
107 |
else |
|
108 |
{ |
|
109 |
//Update where we are. |
|
110 |
dir = UP; |
|
111 |
} |
|
112 |
} |
|
113 |
/* try right */ |
|
114 |
if (map[row][col+1] != WALL && initial_dir != RIGHT) |
|
115 |
{ |
|
116 |
ROS_INFO("GOING RIGHT!"); |
|
117 |
// Turn right. |
|
118 |
turn_from_to(dir, RIGHT); |
|
119 |
follow_line(); |
|
120 |
// Solve recursively. |
|
121 |
bool solved = solve(row, col+1, LEFT); |
|
122 |
if (solved) |
|
123 |
{ |
|
124 |
return solved; |
|
125 |
} |
|
126 |
else |
|
127 |
{ |
|
128 |
//Update where we are. |
|
129 |
dir = RIGHT; |
|
130 |
} |
|
131 |
} |
|
132 |
/* try down */ |
|
133 |
if (map[row+1][col] != WALL && initial_dir != DOWN) |
|
134 |
{ |
|
135 |
ROS_INFO("GOING DOWN!"); |
|
136 |
// Turn down. |
|
137 |
turn_from_to(dir, DOWN); |
|
138 |
follow_line(); |
|
139 |
// Solve recursively. |
|
140 |
bool solved = solve(row+1, col, UP); |
|
141 |
if (solved) |
|
142 |
{ |
|
143 |
return solved; |
|
144 |
} |
|
145 |
else |
|
146 |
{ |
|
147 |
//Update where we are. |
|
148 |
dir = DOWN; |
|
149 |
} |
|
150 |
} |
|
151 |
/* try left */ |
|
152 |
if (map[row][col-1] != WALL && initial_dir != LEFT) |
|
153 |
{ |
|
154 |
ROS_INFO("GOING LEFT!"); |
|
155 |
// Turn down. |
|
156 |
turn_from_to(dir, LEFT); |
|
157 |
follow_line(); |
|
158 |
// Solve recursively. |
|
159 |
bool solved = solve(row, col-1, RIGHT); |
|
160 |
if (solved) |
|
161 |
{ |
|
162 |
return solved; |
|
163 |
} |
|
164 |
else |
|
165 |
{ |
|
166 |
//Update where we are. |
|
167 |
dir = LEFT; |
|
168 |
} |
|
169 |
} |
|
170 |
|
|
171 |
ROS_INFO("DEAD END FOUND, TURNING BACK."); |
|
172 |
// we have exhausted all the options. This path is clearly a |
|
173 |
// dead end. go back to where we come from and return false. |
|
174 |
turn_from_to(dir, initial_dir); |
|
175 |
follow_line(); |
|
176 |
return false; |
|
177 |
} |
|
178 |
|
|
179 |
// this function takes in the current direction and turns the scout |
|
180 |
// into it intended direction |
|
181 |
void maze_solve::turn_from_to(int current_dir, int intended_dir) { |
|
182 |
switch ((4 + intended_dir - current_dir) % 4) |
|
183 |
{ |
|
184 |
case 0: |
|
185 |
spot_turn(); |
|
186 |
break; |
|
187 |
case 1: |
|
188 |
turn_left(); |
|
189 |
break; |
|
190 |
case 2: |
|
191 |
turn_straight(); |
|
192 |
break; |
|
193 |
case 3: |
|
194 |
turn_right(); |
|
195 |
break; |
|
196 |
} |
|
197 |
} |
|
198 |
|
|
199 |
bool maze_solve::look_around(int row, int col, int dir) |
|
200 |
{ |
|
201 |
// look around current place using sonar |
|
202 |
// store whether or not |
|
203 |
// there is a wall into the map |
|
204 |
// stores at row col 2 if point is critical, 1 otherwise |
|
205 |
|
|
206 |
int* readings = sonar->get_sonar_readings(); |
|
207 |
spinOnce(); |
|
208 |
|
|
209 |
/* |
|
210 |
// Look to the left. |
|
211 |
int left_distance = (readings[1] + readings[0] + readings[47])/3; |
|
212 |
// Look to the front. |
|
213 |
int front_distance = (readings[35] + readings[36] + readings[37])/3; |
|
214 |
// Look to the right. |
|
215 |
int right_distance = (readings[23] + readings[24] + readings[25])/3; |
|
216 |
*/ |
|
217 |
// Look to the left. |
|
218 |
int left_distance = readings[0]; |
|
219 |
// Look to the front. |
|
220 |
int front_distance = readings[36]; |
|
221 |
// Look to the right. |
|
222 |
int right_distance = readings[24]; |
|
223 |
|
|
224 |
ROS_INFO("front: %d left: %d right: %d", front_distance, left_distance, right_distance); |
|
225 |
|
|
226 |
if (right_distance == 0 || front_distance == 0 || left_distance == 0) |
|
227 |
return false; |
|
228 |
|
|
229 |
switch (dir) |
|
230 |
{ |
|
231 |
case UP: |
|
232 |
// If the distance is less than 500, mark the area as a wall otherwise |
|
233 |
// mark it as seen. |
|
234 |
map[row][col+1] = (left_distance < 500)?WALL:SEEN; |
|
235 |
map[row+1][col] = (front_distance < 500)?WALL:SEEN; |
|
236 |
map[row][col-1] = (right_distance < 500)?WALL:SEEN; |
|
237 |
break; |
|
238 |
case RIGHT: |
|
239 |
// If the distance is less than 500, mark the area as a wall otherwise |
|
240 |
// mark it as seen. |
|
241 |
map[row+1][col] = (left_distance < 500)?WALL:SEEN; |
|
242 |
map[row][col-1] = (front_distance < 500)?WALL:SEEN; |
|
243 |
map[row-1][col] = (right_distance < 500)?WALL:SEEN; |
|
244 |
break; |
|
245 |
case DOWN: |
|
246 |
// If the distance is less than 500, mark the area as a wall otherwise |
|
247 |
// mark it as seen. |
|
248 |
map[row][col-1] = (left_distance < 500)?WALL:SEEN; |
|
249 |
map[row-1][col] = (front_distance < 500)?WALL:SEEN; |
|
250 |
map[row][col+1] = (right_distance < 500)?WALL:SEEN; |
|
251 |
break; |
|
252 |
case LEFT: |
|
253 |
// If the distance is less than 500, mark the area as a wall otherwise |
|
254 |
// mark it as seen. |
|
255 |
map[row-1][col] = (left_distance < 500)?WALL:SEEN; |
|
256 |
map[row][col+1] = (front_distance < 500)?WALL:SEEN; |
|
257 |
map[row+1][col] = (right_distance < 500)?WALL:SEEN; |
|
258 |
break; |
|
259 |
} |
|
260 |
|
|
261 |
return true; |
|
262 |
} |
|
263 |
|
|
264 |
bool maze_solve::at_destination() |
|
265 |
{ |
|
266 |
vector<uint32_t> readings = linesensor->query(); |
|
267 |
if ( readings[0] > 200 && |
|
268 |
readings[1] < 55 && |
|
269 |
readings[2] < 55 && |
|
270 |
readings[3] > 200 && |
|
271 |
readings[4] > 200 && |
|
272 |
readings[5] < 55 && |
|
273 |
readings[6] < 55 && |
|
274 |
readings[7] > 200 ) |
|
275 |
{ |
|
276 |
return true; |
|
277 |
} |
|
278 |
return false; |
|
279 |
} |
scout/libscout/src/behaviors/maze_solve.h | ||
---|---|---|
1 |
/** |
|
2 |
* Copyright (c) 2011 Colony Project |
|
3 |
* |
|
4 |
* Permission is hereby granted, free of charge, to any person |
|
5 |
* obtaining a copy of this software and associated documentation |
|
6 |
* files (the "Software"), to deal in the Software without |
|
7 |
* restriction, including without limitation the rights to use, |
|
8 |
* copy, modify, merge, publish, distribute, sublicense, and/or sell |
|
9 |
* copies of the Software, and to permit persons to whom the |
|
10 |
* Software is furnished to do so, subject to the following |
|
11 |
* conditions: |
|
12 |
* |
|
13 |
* The above copyright notice and this permission notice shall be |
|
14 |
* included in all copies or substantial portions of the Software. |
|
15 |
* |
|
16 |
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
|
17 |
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES |
|
18 |
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
|
19 |
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT |
|
20 |
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, |
|
21 |
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING |
|
22 |
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
|
23 |
* OTHER DEALINGS IN THE SOFTWARE. |
|
24 |
*/ |
|
25 |
|
|
26 |
#ifndef _MAZE_SOLVE_H_ |
|
27 |
#define _MAZE_SOLVE_H_ |
|
28 |
|
|
29 |
#include "line_follow.h" |
|
30 |
|
|
31 |
class maze_solve: public line_follow |
|
32 |
{ |
|
33 |
public: |
|
34 |
maze_solve(std::string scoutname, Sensors* sensors): |
|
35 |
line_follow(scoutname, "maze_solve", sensors) {}; |
|
36 |
void run(); |
|
37 |
private: |
|
38 |
bool solve(int row, int col, int dir); |
|
39 |
void turn_from_to(int current_dir, int intended_dir); |
|
40 |
bool look_around(int row, int col, int dir); |
|
41 |
bool at_destination(); |
|
42 |
|
|
43 |
int map[60][60]; |
|
44 |
}; |
|
45 |
#endif |
|
46 |
|
scout/libscout/src/behaviors/trafficNavigation.cpp | ||
---|---|---|
1 |
/** |
|
2 |
* Copyright (c) 2011 Colony Project |
|
3 |
* |
|
4 |
* Permission is hereby granted, free of charge, to any person |
|
5 |
* obtaining a copy of this software and associated documentation |
|
6 |
* files (the "Software"), to deal in the Software without |
|
7 |
* restriction, including without limitation the rights to use, |
|
8 |
* copy, modify, merge, publish, distribute, sublicense, and/or sell |
|
9 |
* copies of the Software, and to permit persons to whom the |
|
10 |
* Software is furnished to do so, subject to the following |
|
11 |
* conditions: |
|
12 |
* |
|
13 |
* The above copyright notice and this permission notice shall be |
|
14 |
* included in all copies or substantial portions of the Software. |
|
15 |
* |
|
16 |
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
|
17 |
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES |
|
18 |
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
|
19 |
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT |
|
20 |
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, |
|
21 |
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING |
|
22 |
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
|
23 |
* OTHER DEALINGS IN THE SOFTWARE. |
|
24 |
*/ |
|
25 |
|
|
26 |
#include "trafficNavigation.h" |
|
27 |
#include "../linefollowing/lineDrive.h" |
|
28 |
|
|
29 |
#define DEBUG_INTERSECTION |
|
30 |
#ifdef MAIN_NEW |
|
31 |
|
|
32 |
#ifdef DEBUG_INTERSECTION |
|
33 |
#define DBG_USBS(str) usb_puts(str) |
|
34 |
#define DBG_USBI(x) usb_puti(x) |
|
35 |
#else |
|
36 |
#define DBG_USBS(str) |
|
37 |
#define DBG_USBI(x) |
|
38 |
#endif |
|
39 |
|
|
40 |
static int state, sign, turnDir; |
|
41 |
static char sendBuffer[PACKET_LENGTH], queuePrevBot, queueNextBot, id, nextDir, nextPath, intersectionNum, resolvPrevBotID = -3; |
|
42 |
unsigned char resolvSeed = 0xC9, resolvDraw = 0, resolvPrevBotDraw = 0; |
|
43 |
bool done; |
|
44 |
|
|
45 |
|
|
46 |
//TODO: classes for lineDrive::doDrive and other functions in line_drive.h, clock functions, sending/receiving packets, encoder data, initializations, get ids, etc. |
|
47 |
void trafficNavigation::run() |
|
48 |
{ |
|
49 |
|
|
50 |
/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */ |
|
51 |
|
|
52 |
id = get_robotid(); |
|
53 |
sign = 0; |
|
54 |
delay_ms(500); |
|
55 |
|
|
56 |
//Test code |
|
57 |
state = SROAD; |
|
58 |
|
|
59 |
sendBuffer[1] = id; |
|
60 |
|
|
61 |
|
|
62 |
while (ok()) { |
|
63 |
/*DTN Finite State Machine*/ |
|
64 |
switch(state){ |
|
65 |
case SROAD:/*Following a normal road*/ |
|
66 |
start(); |
|
67 |
done = false; |
|
68 |
|
|
69 |
//finishes when reads a barcode: TODO: how do we detect intersections now? |
|
70 |
while(!done){ |
|
71 |
sign = lineDrive::doDrive(180); |
|
72 |
switch(sign){ |
|
73 |
case NORMAL: |
|
74 |
case FINISHED: |
|
75 |
case LOST: |
|
76 |
case ERROR: |
|
77 |
break; |
|
78 |
default: |
|
79 |
//we have a barcode! |
|
80 |
state = SINTERSECTION_ENTER; |
|
81 |
DBG_USBS("Read Barcode #:"); |
|
82 |
DBG_USBI(sign); |
|
83 |
DBG_USBS("\n"); |
|
84 |
done = true; |
|
85 |
break; |
|
86 |
|
|
87 |
} |
|
88 |
} |
|
89 |
break; |
|
90 |
case SINTERSECTION_ENTER:/*Entering, and in intersection*/ |
|
91 |
stop(); |
|
92 |
lineDrive::doDrive(0); |
|
93 |
|
|
94 |
DBG_USBS("STATE: SINTERSECTION_ENTER\n"); |
|
95 |
|
|
96 |
/*Intersection queue: |
|
97 |
*Each robot when entering the intersection will check for other robots |
|
98 |
*in the intersection, and insert itself in a queue to go through. |
|
99 |
*/ |
|
100 |
queuePrevBot = -1; //the bot that will drive before this bot |
|
101 |
queueNextBot = -1; //the bot that will drive after this bot |
|
102 |
resolvPrevBotID = -3; //in the case of a race, the bot that is |
|
103 |
//to enter the queue before this bot |
|
104 |
resolvPrevBotDraw = 0; //the random priority number that bot has |
|
105 |
resolvDraw = 0; //my random priority number |
|
106 |
|
|
107 |
intersectionNum = getIntersectNum(sign); |
|
108 |
if(intersectionNum == (char) -1){ //invalid |
|
109 |
state = SROAD; |
|
110 |
DBG_USBS("Barcode has invalid intersectionNum\n"); |
|
111 |
break; |
|
112 |
} |
|
113 |
turnDir = validateTurn(sign, getTurnType(sign)); |
|
114 |
if(turnDir == (char) -1){ //invalid |
|
115 |
state = SROAD; |
|
116 |
DBG_USBS("Barcode has invalid turn\n"); |
|
117 |
break; |
|
118 |
} |
|
119 |
|
|
120 |
trafficNavigation::enterIntersection(); //sends wireless packet for entry |
|
121 |
state = SINTERSECTION_WAIT; |
|
122 |
|
|
123 |
rtc_reset(); //reset rtc for timeout wait for reply |
|
124 |
done = false; |
|
125 |
char retried = 0; |
|
126 |
while(rtc_get() < 8 && !done){//waits for a reply, otherwise assumes it is first in queue |
|
127 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER); |
|
128 |
if(rtc_get() > 6 && !retried){//by now all resolvs should be done from bots that arrived earlier... |
|
129 |
trafficNavigation::enterIntersection(); |
|
130 |
retried = 1; |
|
131 |
} |
|
132 |
switch (ret) { |
|
133 |
case KPLACEDINQUEUE: |
|
134 |
done = true; |
|
135 |
break; |
|
136 |
case KFAILEDTOQUEUE: |
|
137 |
DBG_USBS("Failed to queue\n"); |
|
138 |
trafficNavigation::enterIntersection(); |
|
139 |
rtc_reset(); |
|
140 |
break; |
|
141 |
case KRESOLVINGENTER: |
|
142 |
state = SINTERSECTION_ENTER_RESOLV; |
|
143 |
done = true; |
|
144 |
break; |
|
145 |
} |
|
146 |
} |
|
147 |
break; |
|
148 |
|
|
149 |
case SINTERSECTION_ENTER_RESOLV: |
|
150 |
DBG_USBS("STATE: SINTERSECTION_ENTER_RESOLV\n"); |
|
151 |
|
|
152 |
rtc_reset(); |
|
153 |
done = false; |
|
154 |
retried = 0; |
|
155 |
while(rtc_get() < 4 && !done){ |
|
156 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV); |
|
157 |
switch (ret) { |
|
158 |
case KRESOLVINGENTER: |
|
159 |
break; |
|
160 |
case KPLACEDINQUEUE: |
|
161 |
done = true; |
|
162 |
break; |
|
163 |
case KFAILEDTOQUEUE: |
|
164 |
DBG_USBS("Failed to queue\n"); |
|
165 |
trafficNavigation::enterIntersection(); |
|
166 |
rtc_reset(); |
|
167 |
break; |
|
168 |
} |
|
169 |
//if resolvPrevBotID == -1, this indicates that |
|
170 |
//there was a prevbot before, but it has entered |
|
171 |
//the queue so it's our turn. |
|
172 |
if(!done && resolvPrevBotID == (char) -1 && !retried){ |
|
173 |
trafficNavigation::enterIntersection(); |
|
174 |
rtc_reset(); |
|
175 |
retried = 1; |
|
176 |
//if resolvPrevBotID == -2, we have been |
|
177 |
//resolving but never have seen a bot with lower |
|
178 |
//priority than us. after the 6/16ths sec |
|
179 |
//timeout, assume we are first. |
|
180 |
} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 2){ |
|
181 |
//send a intersection reply to myself to |
|
182 |
//trigger other bots to enter queue. |
|
183 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
184 |
sendBuffer[2] = intersectionNum; |
|
185 |
sendBuffer[3] = id; |
|
186 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
187 |
|
|
188 |
done = true; |
|
189 |
break; |
|
190 |
} |
|
191 |
} |
|
192 |
state = SINTERSECTION_WAIT; |
|
193 |
break; |
|
194 |
case SINTERSECTION_WAIT:/*Waiting in intersection */ |
|
195 |
|
|
196 |
|
|
197 |
DBG_USBS("STATE: SINTERSECTION_WAIT\n"); |
|
198 |
|
|
199 |
done = false; |
|
200 |
while(queuePrevBot != (char) -1){ |
|
201 |
done = true; |
|
202 |
int ret = wirelessPacketHandle(state); |
|
203 |
if (ret==KFIRSTINQUEUE) state = SINTERSECTION_DRIVE; |
|
204 |
} |
|
205 |
//hack to make sure bot that just left intersection is |
|
206 |
//really out of the intersection. |
|
207 |
rtc_reset(); |
|
208 |
while(rtc_get() < 2 && done){//wait one second |
|
209 |
wirelessPacketHandle(state); |
|
210 |
} |
|
211 |
|
|
212 |
state = SINTERSECTION_DRIVE; |
|
213 |
break; |
|
214 |
case SINTERSECTION_DRIVE: |
|
215 |
DBG_USBS("STATE: SINTERSECTION_DRIVE\n"); |
|
216 |
start(); |
|
217 |
turn(getIntersectType(sign), turnDir); |
|
218 |
|
|
219 |
|
|
220 |
//Exits intersection |
|
221 |
sendBuffer[0] = WINTERSECTIONEXIT; |
|
222 |
sendBuffer[2] = intersectionNum;//Intersection # |
|
223 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
224 |
|
|
225 |
state = SROAD; |
|
226 |
break; |
|
227 |
default: |
|
228 |
DBG_USBS("I got stuck in an unknown state (Likely highway) My state is "); |
|
229 |
DBG_USBI(state); |
|
230 |
} |
|
231 |
} |
|
232 |
} |
|
233 |
|
|
234 |
int trafficNavigation::wirelessPacketHandle(int state){ |
|
235 |
int dataLength = 0; |
|
236 |
unsigned char *packet = NULL; |
|
237 |
packet = wl_basic_do_default(&dataLength); |
|
238 |
|
|
239 |
/* sanity check */ |
|
240 |
if(dataLength == 0 || packet == NULL) //no packet |
|
241 |
return ENOPACKET; |
|
242 |
|
|
243 |
if(dataLength != PACKET_LENGTH) |
|
244 |
return EPACKETLEN; |
|
245 |
|
|
246 |
DBG_USBS("Recieved Wireless Packet: "); |
|
247 |
for(int i = 0; i < dataLength; i++){ |
|
248 |
DBG_USBI(packet[i]); |
|
249 |
DBG_USBS(" "); |
|
250 |
} |
|
251 |
DBG_USBS("\n"); |
|
252 |
|
|
253 |
switch (packet[0]){ |
|
254 |
case WROADENTRY: //[type, bot, road] |
|
255 |
return ENOACTION; |
|
256 |
break; |
|
257 |
case WROADEXIT: //[type, bot, road] |
|
258 |
return ENOACTION; |
|
259 |
break; |
|
260 |
case WROADSTOP: //[type, bot, road] |
|
261 |
return ENOACTION; |
|
262 |
break; |
|
263 |
case WINTERSECTIONENTRY: //[type, bot, intersection, fromDir, toDir] |
|
264 |
if (packet[2] == intersectionNum){ |
|
265 |
switch (state){ |
|
266 |
case SINTERSECTION_ENTER: |
|
267 |
trafficNavigation::sendResolv(false); |
|
268 |
resolvPrevBotID = -2; |
|
269 |
return KRESOLVINGENTER; |
|
270 |
break; |
|
271 |
case SINTERSECTION_ENTER_RESOLV: |
|
272 |
return ENOACTION; |
|
273 |
case SINTERSECTION_WAIT: |
|
274 |
case SINTERSECTION_DRIVE: |
|
275 |
if(queueNextBot == (char) -1){ |
|
276 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
277 |
sendBuffer[2] = intersectionNum; |
|
278 |
sendBuffer[3] = packet[1]; |
|
279 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
280 |
queueNextBot = packet[1]; |
|
281 |
return KREPLIEDTOENTER; |
|
282 |
} |
|
283 |
break; |
|
284 |
|
|
285 |
} |
|
286 |
} |
|
287 |
break; |
|
288 |
case WINTERSECTIONREPLY: //[type, fromBot, intersection, toBot] |
|
289 |
if (packet[2] == intersectionNum){ |
|
290 |
switch (state){ |
|
291 |
case SINTERSECTION_ENTER: |
|
292 |
case SINTERSECTION_ENTER_RESOLV: |
|
293 |
if(packet[3] == id){ //Reply for me |
|
294 |
queuePrevBot = packet[1]; |
|
295 |
return KPLACEDINQUEUE; |
|
296 |
} else { |
|
297 |
if(packet[3] == resolvPrevBotID) |
|
298 |
resolvPrevBotID = -1; |
|
299 |
return KFAILEDTOQUEUE; |
|
300 |
} |
|
301 |
break; |
|
302 |
default: |
|
303 |
return ENOACTION; |
|
304 |
} |
|
305 |
} |
|
306 |
break; |
|
307 |
case WINTERSECTIONEXIT: //[type, bot, intersection] |
|
308 |
if (packet[2] == intersectionNum){ |
|
309 |
switch (state){ |
|
310 |
case SINTERSECTION_WAIT: |
|
311 |
if(packet[1]==queuePrevBot){ |
|
312 |
queuePrevBot=-1; |
|
313 |
return KFIRSTINQUEUE; |
|
314 |
} |
|
315 |
} |
|
316 |
} |
|
317 |
break; |
|
318 |
case WINTERSECTIONGO: //[type, bot, intersection] |
|
319 |
break; |
|
320 |
case WINTERSECTIONPOLICEENTRY: //[?] |
|
321 |
return ENOACTION; |
|
322 |
break; |
|
323 |
case WINTERSECTIONRESOLVERACE: //[type, bot, intersection, num] |
|
324 |
//in the case robots draw the same number these will be |
|
325 |
//arbitrated using the sending robot's id, prefering |
|
326 |
//lower ids |
|
327 |
DBG_USBS("Now in wireless WINTERSECTIONRESOLVERACE handler: resolvPrevBotID: "); |
|
328 |
DBG_USBI((int) resolvPrevBotID); |
|
329 |
DBG_USBS("\n"); |
|
330 |
if (packet[2] == intersectionNum){ |
|
331 |
switch (state){ |
|
332 |
case SINTERSECTION_ENTER: |
|
333 |
case SINTERSECTION_ENTER_RESOLV: |
|
334 |
if(resolvPrevBotID == (char) -3){ |
|
335 |
DBG_USBS("resolvPrevBotID == -3; sending a resolv packet and setting to -2\n"); |
|
336 |
trafficNavigation::sendResolv(false); |
|
337 |
resolvPrevBotID = -2; |
Also available in: Unified diff