Line data Source code
1 : #include "RobotController.hpp"
2 :
3 : #include "HLR/ObjectMoveStart.h"
4 : #include "InterpolatingAnglePlanner.hpp"
5 : #include "InterpolatingEuclideanPlanner.hpp"
6 :
7 : namespace HLR
8 : {
9 : namespace Kinematics
10 : {
11 0 : RobotController::RobotController(RobotDriverHandler& driver) : driver(driver)
12 : {
13 0 : this->driver.set_move_complete_handler(
14 0 : [this]() { this->position_reached(); });
15 0 : this->events.push(Event::move_to_ready);
16 0 : this->worker = std::thread(&RobotController::event_handler, this);
17 0 : }
18 :
19 0 : RobotController::~RobotController()
20 : {
21 0 : this->events.push(Event::terminate);
22 0 : this->worker.join();
23 0 : }
24 :
25 0 : RobotControllerState RobotController::get_state() const
26 : {
27 0 : std::lock_guard<std::mutex> l(this->state_m);
28 0 : return this->state;
29 : }
30 :
31 0 : void RobotController::pick_up_cup(std::uint32_t id)
32 : {
33 0 : std::lock_guard<std::mutex> l(this->state_m);
34 0 : if (this->state != RobotControllerState::idle)
35 : {
36 0 : throw std::logic_error("Robot is not currently idle.");
37 : }
38 :
39 : {
40 0 : std::lock_guard<std::mutex> l(this->objects_m);
41 0 : const auto it = this->objects.find(id);
42 0 : if (it == this->objects.end())
43 : {
44 : throw std::invalid_argument(
45 0 : "No object found with the specified id.");
46 : }
47 :
48 0 : if (!it->second.is_cup)
49 : {
50 : throw std::invalid_argument(
51 0 : "Specifed object exists but is not a cup.");
52 : }
53 0 : this->cup_position = it->second.position;
54 : }
55 :
56 0 : this->events.push(Event::move_to_cup);
57 0 : }
58 :
59 0 : void RobotController::move_stop()
60 : {
61 0 : std::lock_guard<std::mutex> l(this->state_m);
62 0 : switch (this->state)
63 : {
64 0 : case RobotControllerState::picking_up_cup_stage1:
65 : case RobotControllerState::picking_up_cup_stage2:
66 : case RobotControllerState::moving_to_cup:
67 0 : this->events.push(Event::move_stop);
68 0 : break;
69 0 : default:
70 0 : throw std::logic_error("Robot is is not currently moving.");
71 : }
72 0 : }
73 :
74 0 : void RobotController::move_continue()
75 : {
76 0 : std::lock_guard<std::mutex> l(this->state_m);
77 0 : switch (this->state)
78 : {
79 0 : case RobotControllerState::stopped:
80 0 : this->events.push(Event::move_continue);
81 0 : break;
82 0 : default:
83 0 : throw std::logic_error("Robot is is not currently stopped.");
84 : }
85 0 : }
86 :
87 0 : void RobotController::move_cancel()
88 : {
89 0 : std::lock_guard<std::mutex> l(this->state_m);
90 0 : switch (this->state)
91 : {
92 0 : case RobotControllerState::moving_to_cup:
93 : case RobotControllerState::picking_up_cup_stage1:
94 : case RobotControllerState::picking_up_cup_stage2:
95 : case RobotControllerState::stopped:
96 0 : this->events.push(Event::move_cancel);
97 0 : break;
98 0 : default:
99 : throw std::logic_error(
100 0 : "Robot is is not currently stopped or moving.");
101 : }
102 0 : }
103 :
104 0 : void RobotController::remove_object(const EnvironmentObject& obj)
105 : {
106 0 : std::lock_guard<std::mutex> l(this->objects_m);
107 0 : const auto it = this->objects.find(obj.id);
108 0 : if (it != this->objects.end())
109 : {
110 0 : this->objects.erase(it);
111 : }
112 0 : }
113 :
114 0 : void RobotController::add_object(EnvironmentObject&& obj)
115 : {
116 0 : std::lock_guard<std::mutex> l(this->objects_m);
117 0 : auto it = this->objects.find(obj.id);
118 0 : if (it != this->objects.end())
119 : {
120 0 : auto id = obj.id;
121 0 : this->objects.insert(std::make_pair(id, std::move(obj)));
122 : }
123 : else
124 : {
125 0 : this->objects[obj.id] = obj;
126 : }
127 0 : }
128 :
129 0 : void RobotController::set_error_callback(
130 : const std::function<void(ArmError)>& cb)
131 : {
132 0 : this->error_callback = cb;
133 0 : }
134 :
135 0 : void RobotController::position_reached()
136 : {
137 0 : std::lock_guard<std::mutex> l(this->state_m);
138 0 : switch (this->state)
139 : {
140 0 : case RobotControllerState::moving_to_cup:
141 0 : this->state = RobotControllerState::picking_up_cup_stage1;
142 0 : this->events.push(Event::pick_up_cup_stage1);
143 0 : break;
144 0 : case RobotControllerState::picking_up_cup_stage1:
145 0 : this->state = RobotControllerState::picking_up_cup_stage2;
146 0 : this->events.push(Event::pick_up_cup_stage2);
147 0 : break;
148 0 : case RobotControllerState::picking_up_cup_stage2:
149 0 : return;
150 0 : default:
151 0 : return;
152 : }
153 : }
154 :
155 0 : void RobotController::move_to_position(const Matrix<double, 3, 1>& pos,
156 : const Planner& planner,
157 : double speed)
158 : {
159 0 : auto path_opt = planner.get_path(this->driver.get_arm_state(), pos);
160 :
161 0 : if (!path_opt.has_value() && this->error_callback)
162 : {
163 0 : this->error_callback(ArmError::no_path_found);
164 0 : return;
165 : }
166 :
167 : try
168 : {
169 0 : this->driver.move(path_opt.value(), speed);
170 : }
171 0 : catch (const std::runtime_error& e)
172 : {
173 0 : if (this->error_callback)
174 : {
175 0 : this->error_callback(ArmError::driver_error);
176 : }
177 : }
178 : }
179 :
180 0 : void RobotController::event_handler()
181 : {
182 0 : bool term = false;
183 0 : while (!term)
184 : {
185 0 : auto ev = this->events.pop();
186 0 : switch (ev)
187 : {
188 0 : case Event::move_to_ready:
189 0 : this->move_to_ready_handler();
190 0 : break;
191 0 : case Event::move_to_cup:
192 0 : this->pick_up_cup_handler();
193 0 : break;
194 0 : case Event::pick_up_cup_stage1:
195 0 : this->pick_up_cup_stage1_handler();
196 0 : break;
197 0 : case Event::pick_up_cup_stage2:
198 0 : this->pick_up_cup_stage2_handler();
199 0 : break;
200 0 : case Event::move_stop:
201 0 : this->move_stop_handler();
202 0 : break;
203 0 : case Event::move_continue:
204 0 : this->move_continue_handler();
205 0 : break;
206 0 : case Event::move_cancel:
207 0 : this->move_cancel_handler();
208 0 : break;
209 0 : case Event::terminate:
210 0 : term = true;
211 0 : break;
212 : }
213 : }
214 0 : }
215 :
216 0 : void RobotController::move_to_ready_handler()
217 : {
218 0 : const InterpolatingAnglePlanner planner(this->angle_step_size);
219 0 : const double speed = 20;
220 0 : this->move_to_position(this->ready_pos, planner, speed);
221 0 : }
222 :
223 0 : void RobotController::pick_up_cup_handler()
224 : {
225 0 : const InterpolatingAnglePlanner planner(this->angle_step_size);
226 :
227 : auto path_opt =
228 0 : planner.get_path(this->driver.get_arm_state(), cup_position);
229 :
230 0 : if (!path_opt.has_value() && this->error_callback)
231 : {
232 0 : this->error_callback(ArmError::no_path_found);
233 0 : return;
234 : }
235 :
236 : try
237 : {
238 0 : this->driver.move(path_opt.value(), 50);
239 : }
240 0 : catch (const std::runtime_error& e)
241 : {
242 0 : if (this->error_callback)
243 : {
244 0 : this->error_callback(ArmError::driver_error);
245 : }
246 : }
247 : }
248 :
249 0 : void RobotController::pick_up_cup_stage1_handler()
250 : {
251 0 : const InterpolatingEuclideanPlanner planner(this->euclidean_step_size);
252 :
253 : auto path_opt =
254 0 : planner.get_path(this->driver.get_arm_state(), cup_position);
255 :
256 0 : if (!path_opt.has_value() && this->error_callback)
257 : {
258 0 : this->error_callback(ArmError::no_path_found);
259 0 : return;
260 : }
261 :
262 : try
263 : {
264 0 : this->driver.move(path_opt.value(), 10);
265 : }
266 0 : catch (const std::runtime_error& e)
267 : {
268 0 : if (this->error_callback)
269 : {
270 0 : this->error_callback(ArmError::driver_error);
271 : }
272 : }
273 : }
274 :
275 0 : void RobotController::pick_up_cup_stage2_handler()
276 : {
277 0 : const InterpolatingEuclideanPlanner planner(this->euclidean_step_size);
278 :
279 0 : auto target_pos = cup_position;
280 0 : target_pos[1][0] += 0.10;
281 :
282 0 : auto path_opt = planner.get_path(this->driver.get_arm_state(), target_pos);
283 :
284 0 : if (!path_opt.has_value() && this->error_callback)
285 : {
286 0 : this->error_callback(ArmError::no_path_found);
287 0 : return;
288 : }
289 :
290 : try
291 : {
292 0 : this->driver.move(path_opt.value(), 10);
293 : }
294 0 : catch (const std::runtime_error& e)
295 : {
296 0 : if (this->error_callback)
297 : {
298 0 : this->error_callback(ArmError::driver_error);
299 : }
300 : }
301 : }
302 :
303 0 : void RobotController::move_stop_handler() {}
304 :
305 0 : void RobotController::move_continue_handler() {}
306 :
307 0 : void RobotController::move_cancel_handler() {}
308 :
309 : } // namespace Kinematics
310 3 : } // namespace HLR
|
