root / scout / libscout / src / behaviors / WH_Robot.cpp @ 0e9eb730
History | View | Annotate | Download (4.72 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 | 3f72678f | Priya | WH_Robot::WH_Robot(std::string scoutname):Behavior(scoutname, "WH_Robot") |
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 | ROS_INFO("Path length: %d", path_to_follow.len);
|
||
30 | for(int i=0; i<path_to_follow.len; i++) |
||
31 | { |
||
32 | Turn t = path_to_follow.path[i]; |
||
33 | ROS_INFO("making turn %d at state %d", (int)t, (int)nav_map->get_state()); |
||
34 | 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(); |
||
38 | } |
||
39 | ROS_INFO("Path complete at state %d", (int)nav_map->get_state()); |
||
40 | } |
||
41 | |||
42 | 9b4328d7 | Priya | int WH_Robot::exec_task()
|
43 | bebd9bcb | Leon | { |
44 | ad5b5826 | Priya | ROS_INFO("WH_robot: Executing a task");
|
45 | assert(curr_task != DEFAULT_TASK); |
||
46 | 5d0687a9 | Priya | |
47 | d37cfc69 | Leon | // remember where you started doing this task so we know where to return
|
48 | 0e9eb730 | Leon | State home_state = nav_map->get_state(); |
49 | d37cfc69 | Leon | ROS_INFO("WH_robot: starting from home state %d", (int)home_state); |
50 | 0e9eb730 | Leon | ROS_INFO("WH_robot: aiming for state %d", (int)curr_task->get_dest()); |
51 | 5d0687a9 | Priya | Path new_path = nav_map->shortest_path(curr_task->get_dest()); |
52 | curr_task->set_path(new_path); |
||
53 | d37cfc69 | Leon | |
54 | 0e9eb730 | Leon | follow_path(new_path); |
55 | d37cfc69 | Leon | |
56 | //TODO: forklift yaaaay
|
||
57 | |||
58 | int did_task_complete;
|
||
59 | |||
60 | ad5b5826 | Priya | srand(0xDEADBEEF);
|
61 | int error = rand() % 12; |
||
62 | 5d0687a9 | Priya | if(error < 6) //Fail with 1/2 probability |
63 | ad5b5826 | Priya | { |
64 | ROS_INFO("WH_robot: TASK COMPLETE!");
|
||
65 | d37cfc69 | Leon | did_task_complete = TASK_COMPLETED; |
66 | ad5b5826 | Priya | } |
67 | else
|
||
68 | { |
||
69 | ROS_INFO("WH_robot: TASK FAILED!");
|
||
70 | d37cfc69 | Leon | did_task_complete = TASK_FAILED; |
71 | ad5b5826 | Priya | } |
72 | d37cfc69 | Leon | |
73 | ROS_INFO("WH_robot: Now I am attempting to go home from state %d to state %d",
|
||
74 | (int)nav_map->get_state(), (int)home_state); |
||
75 | |||
76 | Path return_path = nav_map->shortest_path(home_state); |
||
77 | curr_task->set_path(return_path); |
||
78 | |||
79 | follow_path(return_path); |
||
80 | |||
81 | return did_task_complete;
|
||
82 | } |
||
83 | |||
84 | 7ac5e9bc | Priya | void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg) |
85 | 3f72678f | Priya | { |
86 | ad5b5826 | Priya | std::cout << "robot callback called." << std::endl;
|
87 | 3f72678f | Priya | if(msg->data.compare(0, 11, "REG_SUCCESS") == 0) |
88 | { |
||
89 | 7ac5e9bc | Priya | id = atoi(msg->data.substr(12).c_str());
|
90 | 3f72678f | Priya | reg_failed = 0;
|
91 | } |
||
92 | else if(msg->data.compare(0, 8, "SET_TASK") == 0) |
||
93 | { |
||
94 | ad5b5826 | Priya | ROS_INFO("WH_ROBOT: Setting robot task");
|
95 | 7ac5e9bc | Priya | char* string = (char*)msg->data.c_str(); |
96 | 3f72678f | Priya | char* data;
|
97 | data = strtok(string, " "); |
||
98 | 7ac5e9bc | Priya | int order_id = atoi(data);
|
99 | 3f72678f | Priya | data = strtok(string, " "); |
100 | Address source = atoi(data); |
||
101 | data = strtok(string, " "); |
||
102 | Address dest = atoi(data); |
||
103 | Time start_time = ros::Time::now(); |
||
104 | 7ac5e9bc | Priya | Path path; |
105 | path.len = 0;
|
||
106 | path.path = NULL;
|
||
107 | 3f72678f | Priya | Duration est_time(0);
|
108 | |||
109 | 7ac5e9bc | Priya | curr_task = new Order(order_id, source, dest, start_time, path, est_time);
|
110 | 3f72678f | Priya | } |
111 | else
|
||
112 | { |
||
113 | 7ac5e9bc | Priya | ROS_INFO("I got a bad message: %s", msg->data.c_str());
|
114 | 3f72678f | Priya | } |
115 | } |
||
116 | |||
117 | void WH_Robot::run ()
|
||
118 | { |
||
119 | ad5b5826 | Priya | ROS_INFO("A WH_Robot has been created");
|
120 | |||
121 | robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000); |
||
122 | sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this); |
||
123 | |||
124 | ROS_INFO("I am a robot. Registering with scheduler...");
|
||
125 | while(reg_failed && ok())
|
||
126 | { |
||
127 | std_msgs::String msg; |
||
128 | std::stringstream ss; |
||
129 | ss << "REGISTER "<<name;
|
||
130 | msg.data = ss.str(); |
||
131 | robot_to_sched.publish(msg); |
||
132 | |||
133 | spinOnce(); |
||
134 | } |
||
135 | |||
136 | while(ok())
|
||
137 | { |
||
138 | ROS_INFO("WH_ROBOT: Running main loop");
|
||
139 | |||
140 | |||
141 | 7ac5e9bc | Priya | std_msgs::String msg; |
142 | std::stringstream ss; |
||
143 | ad5b5826 | Priya | ss << "GET_TASK "<<id;
|
144 | 7ac5e9bc | Priya | msg.data = ss.str(); |
145 | robot_to_sched.publish(msg); |
||
146 | |||
147 | ad5b5826 | Priya | while(curr_task == DEFAULT_TASK && ok()){
|
148 | 7ac5e9bc | Priya | spinOnce(); |
149 | } |
||
150 | 3f72678f | Priya | |
151 | ad5b5826 | Priya | int error = exec_task();
|
152 | if(error == TASK_COMPLETED)
|
||
153 | bebd9bcb | Leon | { |
154 | ad5b5826 | Priya | std_msgs::String msg; |
155 | std::stringstream ss; |
||
156 | ss << "SUCCESS "<<curr_task->getid();
|
||
157 | msg.data = ss.str(); |
||
158 | robot_to_sched.publish(msg); |
||
159 | bebd9bcb | Leon | } |
160 | ad5b5826 | Priya | else //error == TASK_FAILED |
161 | { |
||
162 | std_msgs::String msg; |
||
163 | std::stringstream ss; |
||
164 | ss << "FAILED "<<curr_task->getid();
|
||
165 | msg.data = ss.str(); |
||
166 | robot_to_sched.publish(msg); |
||
167 | } |
||
168 | delete curr_task;
|
||
169 | curr_task = DEFAULT_TASK; |
||
170 | |||
171 | } |
||
172 | bebd9bcb | Leon | } |
173 | |||
174 | 9b4328d7 | Priya | void WH_Robot::set_task(Order order)
|
175 | { |
||
176 | ad5b5826 | Priya | curr_task = new Order(order.getid(), order.get_source(), order.get_dest(),
|
177 | order.get_start_time(), order.get_path(), order.get_est_time()); |
||
178 | return;
|
||
179 | bebd9bcb | Leon | } |
180 | 9b4328d7 | Priya |