Line data Source code
1 : #include "RobotController.hpp"
2 :
3 : #include "HLR/ObjectMoveStart.h"
4 : #include "InterpolatingAnglePlanner.hpp"
5 :
6 : namespace HLR
7 : {
8 : namespace Kinematics
9 : {
10 0 : RobotController::RobotController(RobotDriverHandler& driver) : driver(driver)
11 : {
12 0 : this->events.push(Event::move_to_ready);
13 0 : this->worker = std::thread(&RobotController::event_handler, this);
14 0 : }
15 :
16 0 : RobotController::~RobotController()
17 : {
18 0 : this->events.push(Event::terminate);
19 0 : this->worker.join();
20 0 : }
21 :
22 0 : RobotControllerState RobotController::get_state() const
23 : {
24 0 : std::lock_guard<std::mutex> l(this->state_m);
25 0 : return this->state;
26 : }
27 :
28 0 : void RobotController::pick_up_cup(std::uint32_t id)
29 : {
30 0 : std::lock_guard<std::mutex> l(this->state_m);
31 0 : if (this->state != RobotControllerState::idle)
32 : {
33 0 : throw std::logic_error("Robot is not currently idle.");
34 : }
35 :
36 : {
37 0 : std::lock_guard<std::mutex> l(this->objects_m);
38 0 : const auto it = this->objects.find(id);
39 0 : if (it == this->objects.end())
40 : {
41 : throw std::invalid_argument(
42 0 : "No object found with the specified id.");
43 : }
44 :
45 0 : if (!it->second.is_cup)
46 : {
47 : throw std::invalid_argument(
48 0 : "Specifed object exists but is not a cup.");
49 : }
50 0 : this->cup_position = it->second.position;
51 : }
52 :
53 0 : this->events.push(Event::pick_up_cup);
54 0 : }
55 :
56 0 : void RobotController::move_stop()
57 : {
58 0 : std::lock_guard<std::mutex> l(this->state_m);
59 0 : switch (this->state)
60 : {
61 0 : case RobotControllerState::moving_to_cup:
62 0 : this->events.push(Event::move_stop);
63 0 : break;
64 0 : default:
65 0 : throw std::logic_error("Robot is is not currently moving.");
66 : }
67 0 : }
68 :
69 0 : void RobotController::move_continue()
70 : {
71 0 : std::lock_guard<std::mutex> l(this->state_m);
72 0 : switch (this->state)
73 : {
74 0 : case RobotControllerState::stopped:
75 0 : this->events.push(Event::move_continue);
76 0 : break;
77 0 : default:
78 0 : throw std::logic_error("Robot is is not currently stopped.");
79 : }
80 0 : }
81 :
82 0 : void RobotController::move_cancel()
83 : {
84 0 : std::lock_guard<std::mutex> l(this->state_m);
85 0 : switch (this->state)
86 : {
87 0 : case RobotControllerState::moving_to_cup:
88 : case RobotControllerState::stopped:
89 0 : this->events.push(Event::move_cancel);
90 0 : break;
91 0 : default:
92 : throw std::logic_error(
93 0 : "Robot is is not currently stopped or moving.");
94 : }
95 0 : }
96 :
97 0 : void RobotController::remove_object(const EnvironmentObject& obj)
98 : {
99 0 : std::lock_guard<std::mutex> l(this->objects_m);
100 0 : const auto it = this->objects.find(obj.id);
101 0 : if (it != this->objects.end())
102 : {
103 0 : this->objects.erase(it);
104 : }
105 0 : }
106 :
107 0 : void RobotController::add_object(EnvironmentObject&& obj)
108 : {
109 0 : std::lock_guard<std::mutex> l(this->objects_m);
110 0 : auto it = this->objects.find(obj.id);
111 0 : if (it != this->objects.end())
112 : {
113 0 : auto id = obj.id;
114 0 : this->objects.insert(std::make_pair(id, std::move(obj)));
115 : }
116 : else
117 : {
118 0 : this->objects[obj.id] = obj;
119 : }
120 0 : }
121 :
122 0 : void RobotController::set_error_callback(
123 : const std::function<void(ArmError)>& cb)
124 : {
125 0 : this->error_callback = cb;
126 0 : }
127 :
128 0 : void RobotController::move_to_position(const Matrix<double, 3, 1>& pos,
129 : const Planner& planner,
130 : double speed)
131 : {
132 0 : auto path_opt = planner.get_path(this->driver.get_arm_state(), pos);
133 :
134 0 : if (!path_opt.has_value() && this->error_callback)
135 : {
136 0 : this->error_callback(ArmError::no_path_found);
137 0 : return;
138 : }
139 :
140 : try
141 : {
142 0 : this->driver.move(path_opt.value(), speed);
143 : }
144 0 : catch (const std::runtime_error& e)
145 : {
146 0 : if (this->error_callback)
147 : {
148 0 : this->error_callback(ArmError::driver_error);
149 : }
150 : }
151 : }
152 :
153 0 : void RobotController::event_handler()
154 : {
155 0 : bool term = false;
156 0 : while (!term)
157 : {
158 0 : auto ev = this->events.pop();
159 0 : switch (ev)
160 : {
161 0 : case Event::move_to_ready:
162 0 : this->move_to_ready_handler();
163 0 : break;
164 0 : case Event::pick_up_cup:
165 0 : this->pick_up_cup_handler();
166 0 : break;
167 0 : case Event::move_stop:
168 0 : this->move_stop_handler();
169 0 : break;
170 0 : case Event::move_continue:
171 0 : this->move_continue_handler();
172 0 : break;
173 0 : case Event::move_cancel:
174 0 : this->move_cancel_handler();
175 0 : break;
176 0 : case Event::arm_move_complete:
177 0 : this->arm_move_complete_handler();
178 0 : break;
179 0 : case Event::terminate:
180 0 : term = true;
181 0 : break;
182 : }
183 : }
184 0 : }
185 :
186 0 : void RobotController::move_to_ready_handler()
187 : {
188 0 : const InterpolatingAnglePlanner planner(this->angle_step_size);
189 0 : const double speed = 20;
190 0 : this->move_to_position(this->ready_pos, planner, speed);
191 0 : }
192 :
193 0 : void RobotController::pick_up_cup_handler()
194 : {
195 0 : const InterpolatingAnglePlanner planner(this->angle_step_size);
196 :
197 : auto path_opt =
198 0 : planner.get_path(this->driver.get_arm_state(), cup_position);
199 :
200 0 : if (!path_opt.has_value() && this->error_callback)
201 : {
202 0 : this->error_callback(ArmError::no_path_found);
203 0 : return;
204 : }
205 :
206 : try
207 : {
208 0 : this->driver.move(path_opt.value(), 50);
209 : }
210 0 : catch (const std::runtime_error& e)
211 : {
212 0 : if (this->error_callback)
213 : {
214 0 : this->error_callback(ArmError::driver_error);
215 : }
216 : }
217 : }
218 :
219 0 : void RobotController::arm_move_complete_handler() {}
220 :
221 0 : void RobotController::move_stop_handler() {}
222 :
223 0 : void RobotController::move_continue_handler() {}
224 :
225 0 : void RobotController::move_cancel_handler() {}
226 :
227 : } // namespace Kinematics
228 3 : } // namespace HLR
|
