scoutos / scout / libscout / src / behaviors / WH_Robot.cpp @ afa9104d
History | View | Annotate | Download (5.06 KB)
1 | bebd9bcb | Leon | #include "WH_Robot.h" |
---|---|---|---|
2 | 6761a531 | Priya | #include "../helper_classes/Order.h" |
3 | bebd9bcb | Leon | |
4 | /** @Brief: warehouse robot constructor **/
|
||
5 | 1905324e | Priya | WH_Robot::WH_Robot(std::string scoutname):line_follow(scoutname)
|
6 | bebd9bcb | Leon | { |
7 | ad5b5826 | Priya | nav_map = new navigationMap(scoutname);
|
8 | curr_task = DEFAULT_TASK; |
||
9 | name = scoutname; |
||
10 | reg_failed = 1;
|
||
11 | bebd9bcb | Leon | } |
12 | |||
13 | 6761a531 | Priya | WH_Robot::~WH_Robot() |
14 | { |
||
15 | ad5b5826 | Priya | delete(nav_map);
|
16 | 6761a531 | Priya | } |
17 | |||
18 | Duration WH_Robot::get_worst_case_time(State start_state, State target_state) |
||
19 | bebd9bcb | Leon | { |
20 | 6761a531 | Priya | return nav_map->get_worst_case_time(start_state, target_state);
|
21 | bebd9bcb | Leon | } |
22 | |||
23 | 0e9eb730 | Leon | /** @brief Based on the given path, make
|
24 | * the series of turns to follow this path,
|
||
25 | * updating the current state in the navigation map as we do so
|
||
26 | * @param the Path to follow
|
||
27 | */
|
||
28 | void WH_Robot::follow_path(Path path_to_follow){
|
||
29 | for(int i=0; i<path_to_follow.len; i++) |
||
30 | { |
||
31 | Turn t = path_to_follow.path[i]; |
||
32 | 1905324e | Priya | ROS_INFO("making turn %d", t);
|
33 | 0e9eb730 | Leon | nav_map->update_state(t); |
34 | 1905324e | Priya | switch(t)
|
35 | { |
||
36 | case ISTRAIGHT:
|
||
37 | afa9104d | Priya | turn_straight(); |
38 | 1905324e | Priya | follow_line(); |
39 | break;
|
||
40 | case ILEFT:
|
||
41 | turn_left(); |
||
42 | follow_line(); |
||
43 | break;
|
||
44 | case IRIGHT:
|
||
45 | turn_right(); |
||
46 | follow_line(); |
||
47 | break;
|
||
48 | case IUTURN:
|
||
49 | u_turn(); |
||
50 | follow_line(); |
||
51 | break;
|
||
52 | } |
||
53 | 0e9eb730 | Leon | } |
54 | } |
||
55 | |||
56 | 9b4328d7 | Priya | int WH_Robot::exec_task()
|
57 | bebd9bcb | Leon | { |
58 | ad5b5826 | Priya | ROS_INFO("WH_robot: Executing a task");
|
59 | assert(curr_task != DEFAULT_TASK); |
||
60 | 5d0687a9 | Priya | |
61 | d37cfc69 | Leon | // remember where you started doing this task so we know where to return
|
62 | 0e9eb730 | Leon | State home_state = nav_map->get_state(); |
63 | d37cfc69 | Leon | ROS_INFO("WH_robot: starting from home state %d", (int)home_state); |
64 | 0e9eb730 | Leon | ROS_INFO("WH_robot: aiming for state %d", (int)curr_task->get_dest()); |
65 | 5d0687a9 | Priya | Path new_path = nav_map->shortest_path(curr_task->get_dest()); |
66 | curr_task->set_path(new_path); |
||
67 | d37cfc69 | Leon | |
68 | 0e9eb730 | Leon | follow_path(new_path); |
69 | d37cfc69 | Leon | |
70 | //TODO: forklift yaaaay
|
||
71 | |||
72 | int did_task_complete;
|
||
73 | |||
74 | ad5b5826 | Priya | srand(0xDEADBEEF);
|
75 | int error = rand() % 12; |
||
76 | 5d0687a9 | Priya | if(error < 6) //Fail with 1/2 probability |
77 | ad5b5826 | Priya | { |
78 | ROS_INFO("WH_robot: TASK COMPLETE!");
|
||
79 | d37cfc69 | Leon | did_task_complete = TASK_COMPLETED; |
80 | ad5b5826 | Priya | } |
81 | else
|
||
82 | { |
||
83 | ROS_INFO("WH_robot: TASK FAILED!");
|
||
84 | d37cfc69 | Leon | did_task_complete = TASK_FAILED; |
85 | ad5b5826 | Priya | } |
86 | d37cfc69 | Leon | |
87 | ROS_INFO("WH_robot: Now I am attempting to go home from state %d to state %d",
|
||
88 | (int)nav_map->get_state(), (int)home_state); |
||
89 | |||
90 | Path return_path = nav_map->shortest_path(home_state); |
||
91 | curr_task->set_path(return_path); |
||
92 | |||
93 | follow_path(return_path); |
||
94 | |||
95 | return did_task_complete;
|
||
96 | } |
||
97 | |||
98 | 7ac5e9bc | Priya | void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg) |
99 | 3f72678f | Priya | { |
100 | ad5b5826 | Priya | std::cout << "robot callback called." << std::endl;
|
101 | 3f72678f | Priya | if(msg->data.compare(0, 11, "REG_SUCCESS") == 0) |
102 | { |
||
103 | 7ac5e9bc | Priya | id = atoi(msg->data.substr(12).c_str());
|
104 | 3f72678f | Priya | reg_failed = 0;
|
105 | } |
||
106 | else if(msg->data.compare(0, 8, "SET_TASK") == 0) |
||
107 | { |
||
108 | ad5b5826 | Priya | ROS_INFO("WH_ROBOT: Setting robot task");
|
109 | 7ac5e9bc | Priya | char* string = (char*)msg->data.c_str(); |
110 | 351f71d1 | Priya | ROS_INFO("Set task string is: %s", string); |
111 | 3f72678f | Priya | char* data;
|
112 | data = strtok(string, " "); |
||
113 | 351f71d1 | Priya | data = strtok(NULL, " "); |
114 | 7ac5e9bc | Priya | int order_id = atoi(data);
|
115 | 351f71d1 | Priya | ROS_INFO("order Id: %d with string %s", order_id, data);
|
116 | |||
117 | data = strtok(NULL, " "); |
||
118 | 3f72678f | Priya | Address source = atoi(data); |
119 | 351f71d1 | Priya | ROS_INFO("source: %d with string %s", source, data);
|
120 | |||
121 | data = strtok(NULL, " "); |
||
122 | 3f72678f | Priya | Address dest = atoi(data); |
123 | 351f71d1 | Priya | ROS_INFO("dest: %d with string %s", dest, data);
|
124 | |||
125 | 3f72678f | Priya | Time start_time = ros::Time::now(); |
126 | 7ac5e9bc | Priya | Path path; |
127 | path.len = 0;
|
||
128 | path.path = NULL;
|
||
129 | 3f72678f | Priya | Duration est_time(0);
|
130 | |||
131 | 7ac5e9bc | Priya | curr_task = new Order(order_id, source, dest, start_time, path, est_time);
|
132 | 3f72678f | Priya | } |
133 | else
|
||
134 | { |
||
135 | 7ac5e9bc | Priya | ROS_INFO("I got a bad message: %s", msg->data.c_str());
|
136 | 3f72678f | Priya | } |
137 | } |
||
138 | |||
139 | void WH_Robot::run ()
|
||
140 | { |
||
141 | ad5b5826 | Priya | ROS_INFO("A WH_Robot has been created");
|
142 | |||
143 | robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000); |
||
144 | sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this); |
||
145 | |||
146 | ROS_INFO("I am a robot. Registering with scheduler...");
|
||
147 | while(reg_failed && ok())
|
||
148 | { |
||
149 | std_msgs::String msg; |
||
150 | std::stringstream ss; |
||
151 | ss << "REGISTER "<<name;
|
||
152 | msg.data = ss.str(); |
||
153 | robot_to_sched.publish(msg); |
||
154 | |||
155 | spinOnce(); |
||
156 | } |
||
157 | |||
158 | 1905324e | Priya | follow_line(); |
159 | |||
160 | ad5b5826 | Priya | while(ok())
|
161 | { |
||
162 | ROS_INFO("WH_ROBOT: Running main loop");
|
||
163 | |||
164 | |||
165 | 7ac5e9bc | Priya | std_msgs::String msg; |
166 | std::stringstream ss; |
||
167 | ad5b5826 | Priya | ss << "GET_TASK "<<id;
|
168 | 7ac5e9bc | Priya | msg.data = ss.str(); |
169 | robot_to_sched.publish(msg); |
||
170 | |||
171 | ad5b5826 | Priya | while(curr_task == DEFAULT_TASK && ok()){
|
172 | 7ac5e9bc | Priya | spinOnce(); |
173 | } |
||
174 | 3f72678f | Priya | |
175 | ad5b5826 | Priya | int error = exec_task();
|
176 | if(error == TASK_COMPLETED)
|
||
177 | bebd9bcb | Leon | { |
178 | ad5b5826 | Priya | std_msgs::String msg; |
179 | std::stringstream ss; |
||
180 | ss << "SUCCESS "<<curr_task->getid();
|
||
181 | msg.data = ss.str(); |
||
182 | robot_to_sched.publish(msg); |
||
183 | bebd9bcb | Leon | } |
184 | ad5b5826 | Priya | else //error == TASK_FAILED |
185 | { |
||
186 | std_msgs::String msg; |
||
187 | std::stringstream ss; |
||
188 | ss << "FAILED "<<curr_task->getid();
|
||
189 | msg.data = ss.str(); |
||
190 | robot_to_sched.publish(msg); |
||
191 | } |
||
192 | delete curr_task;
|
||
193 | curr_task = DEFAULT_TASK; |
||
194 | |||
195 | } |
||
196 | bebd9bcb | Leon | } |
197 | |||
198 | 9b4328d7 | Priya | void WH_Robot::set_task(Order order)
|
199 | { |
||
200 | ad5b5826 | Priya | curr_task = new Order(order.getid(), order.get_source(), order.get_dest(),
|
201 | order.get_start_time(), order.get_path(), order.get_est_time()); |
||
202 | return;
|
||
203 | bebd9bcb | Leon | } |
204 | 9b4328d7 | Priya |