Line data Source code
1 : #include "ObjectMoveHandler.hpp"
2 :
3 : #include <chrono>
4 : #include <cstdint>
5 :
6 : namespace HLR
7 : {
8 : namespace Kinematics
9 : {
10 : // also defined in ROS msg
11 : const static uint8_t msg_ok = 0;
12 : const static uint8_t msg_state = 1;
13 :
14 2 : ObjectMoveHandler::ObjectMoveHandler()
15 : {
16 : // bind topics and services
17 2 : progress_pub = nh.advertise<HLR::Progress>("move_progress", 1000);
18 4 : service_move_start = nh.advertiseService(
19 2 : "move_start", &ObjectMoveHandler::object_move_start, this);
20 4 : service_move_cancel = nh.advertiseService(
21 2 : "move_cancel", &ObjectMoveHandler::object_move_cancel, this);
22 4 : service_move_continue = nh.advertiseService(
23 2 : "move_continue", &ObjectMoveHandler::object_move_continue, this);
24 4 : service_move_stop = nh.advertiseService(
25 2 : "move_stop", &ObjectMoveHandler::object_move_stop, this);
26 :
27 : // spawn worker thread
28 4 : worker_thread = std::thread([this] { object_move_worker(); });
29 2 : }
30 :
31 5 : ObjectMoveHandler::~ObjectMoveHandler()
32 : {
33 : // stop worker thread
34 2 : worker_state_m.lock();
35 2 : worker_state = State::exit;
36 2 : worker_state_m.unlock();
37 2 : queue_m.lock();
38 2 : cv.notify_all();
39 2 : queue_m.unlock();
40 2 : worker_thread.join();
41 3 : }
42 :
43 2 : void ObjectMoveHandler::object_move_worker()
44 : {
45 : State state;
46 2 : const std::chrono::milliseconds sleep_for(200);
47 4 : std::unique_lock<std::mutex> cv_lock(queue_m, std::defer_lock);
48 :
49 : while (true)
50 : {
51 5 : worker_state_m.lock();
52 5 : worker_state = State::idle;
53 5 : worker_state_m.unlock();
54 :
55 : // wait for a cup to be queued or exit when requested
56 5 : cv_lock.lock();
57 28 : cv.wait(cv_lock, [this] {
58 16 : std::lock_guard<std::mutex> l(worker_state_m);
59 8 : if (worker_state == State::exit)
60 : {
61 1 : return true;
62 : }
63 :
64 7 : return !queue.empty();
65 : });
66 5 : cv_lock.unlock();
67 :
68 : // exit when requested, reset state otherwise
69 5 : worker_state_m.lock();
70 5 : if (worker_state == State::exit)
71 : {
72 1 : worker_state_m.unlock();
73 1 : return;
74 : }
75 4 : worker_state = State::cont;
76 4 : worker_state_m.unlock();
77 :
78 : // pop the cup
79 4 : queue_m.lock();
80 4 : queue.pop();
81 4 : queue_m.unlock();
82 :
83 4 : progress.progress = 0;
84 4 : progress_pub.publish(progress);
85 :
86 91 : do
87 : {
88 : // exit when requested, handle external state changes
89 95 : worker_state_m.lock();
90 95 : if (worker_state == State::exit)
91 : {
92 1 : worker_state_m.unlock();
93 1 : return;
94 : }
95 94 : state = worker_state;
96 94 : worker_state_m.unlock();
97 :
98 94 : switch (state)
99 : {
100 58 : case State::cont:
101 : // ROS' internal type causes issues with Wconversion
102 : // 0 <= progress <= 100 so this is safe
103 58 : progress.progress =
104 58 : static_cast<uint8_t>(progress.progress + 5);
105 58 : progress_pub.publish(progress);
106 58 : break;
107 :
108 4 : case State::exit:
109 : case State::idle:
110 : case State::stop:
111 4 : break;
112 :
113 32 : case State::cancel:
114 : // ROS' internal type causes issues with Wconversion
115 : // 0 <= progress <= 100 so this is safe
116 32 : progress.progress =
117 32 : static_cast<uint8_t>(progress.progress - 5);
118 32 : progress_pub.publish(progress);
119 32 : break;
120 : }
121 :
122 : // simulate the arm actually moving by sleeping a bit
123 : // also prevents async service/topic handling bugs during tests
124 94 : std::this_thread::sleep_for(sleep_for);
125 94 : } while (progress.progress < 100
126 94 : && !(state == State::cancel && progress.progress == 0));
127 3 : }
128 : }
129 :
130 4 : bool ObjectMoveHandler::object_move_start(HLR::ObjectMove::Request& req,
131 : HLR::ObjectMove::Response& res)
132 : {
133 4 : queue_m.lock();
134 4 : queue.emplace(req.request);
135 4 : cv.notify_one();
136 4 : queue_m.unlock();
137 :
138 4 : res.response = msg_ok;
139 4 : return true;
140 : }
141 :
142 7 : bool ObjectMoveHandler::object_move_cancel(
143 : HLR::ObjectMove::Request& req[[maybe_unused]],
144 : HLR::ObjectMove::Response& res)
145 : {
146 14 : std::lock_guard<std::mutex> l(worker_state_m);
147 :
148 7 : if (worker_state != State::stop)
149 : {
150 5 : res.response = msg_state;
151 5 : return false;
152 : }
153 2 : worker_state = State::cancel;
154 :
155 2 : res.response = msg_ok;
156 2 : return true;
157 : }
158 :
159 7 : bool ObjectMoveHandler::object_move_continue(
160 : HLR::ObjectMove::Request& req[[maybe_unused]],
161 : HLR::ObjectMove::Response& res)
162 : {
163 14 : std::lock_guard<std::mutex> l(worker_state_m);
164 :
165 7 : if (worker_state != State::stop)
166 : {
167 5 : res.response = msg_state;
168 5 : return false;
169 : }
170 2 : worker_state = State::cont;
171 :
172 2 : res.response = msg_ok;
173 2 : return true;
174 : }
175 :
176 9 : bool ObjectMoveHandler::object_move_stop(
177 : HLR::ObjectMove::Request& req[[maybe_unused]],
178 : HLR::ObjectMove::Response& res)
179 : {
180 18 : std::lock_guard<std::mutex> l(worker_state_m);
181 :
182 9 : if (worker_state != State::cont)
183 : {
184 5 : res.response = msg_state;
185 5 : return false;
186 : }
187 4 : worker_state = State::stop;
188 :
189 4 : res.response = msg_ok;
190 4 : return true;
191 : }
192 :
193 : } // namespace Kinematics
194 3 : } // namespace HLR
|
