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(10);
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 97 : do
87 : {
88 : // exit when requested, handle external state changes
89 101 : worker_state_m.lock();
90 101 : if (worker_state == State::exit)
91 : {
92 1 : worker_state_m.unlock();
93 1 : return;
94 : }
95 100 : state = worker_state;
96 100 : worker_state_m.unlock();
97 :
98 100 : 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 10 : case State::exit:
109 : case State::idle:
110 : case State::stop:
111 10 : 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 100 : std::this_thread::sleep_for(sleep_for);
124 100 : } while (progress.progress < 100
125 100 : && !(state == State::cancel && progress.progress == 0));
126 3 : }
127 : }
128 :
129 4 : bool ObjectMoveHandler::object_move_start(HLR::ObjectMove::Request& req,
130 : HLR::ObjectMove::Response& res)
131 : {
132 4 : queue_m.lock();
133 4 : queue.emplace(req.request);
134 4 : cv.notify_one();
135 4 : queue_m.unlock();
136 :
137 4 : res.response = msg_ok;
138 4 : return true;
139 : }
140 :
141 7 : bool ObjectMoveHandler::object_move_cancel(
142 : HLR::ObjectMove::Request& req[[maybe_unused]],
143 : HLR::ObjectMove::Response& res)
144 : {
145 14 : std::lock_guard<std::mutex> l(worker_state_m);
146 :
147 7 : if (worker_state != State::stop)
148 : {
149 5 : res.response = msg_state;
150 5 : return false;
151 : }
152 2 : worker_state = State::cancel;
153 :
154 2 : res.response = msg_ok;
155 2 : return true;
156 : }
157 :
158 7 : bool ObjectMoveHandler::object_move_continue(
159 : HLR::ObjectMove::Request& req[[maybe_unused]],
160 : HLR::ObjectMove::Response& res)
161 : {
162 14 : std::lock_guard<std::mutex> l(worker_state_m);
163 :
164 7 : if (worker_state != State::stop)
165 : {
166 5 : res.response = msg_state;
167 5 : return false;
168 : }
169 2 : worker_state = State::cont;
170 :
171 2 : res.response = msg_ok;
172 2 : return true;
173 : }
174 :
175 9 : bool ObjectMoveHandler::object_move_stop(
176 : HLR::ObjectMove::Request& req[[maybe_unused]],
177 : HLR::ObjectMove::Response& res)
178 : {
179 18 : std::lock_guard<std::mutex> l(worker_state_m);
180 :
181 9 : if (worker_state != State::cont)
182 : {
183 5 : res.response = msg_state;
184 5 : return false;
185 : }
186 4 : worker_state = State::stop;
187 :
188 4 : res.response = msg_ok;
189 4 : return true;
190 : }
191 :
192 : } // namespace Kinematics
193 3 : } // namespace HLR
|
