Line data Source code
1 : #include "Driver.hpp"
2 :
3 : #include <chrono>
4 : #include <cstdint>
5 : #include <sstream>
6 :
7 : // robot ID and slot are both set to 1
8 : // http://94.124.143.236/confluence/display/FRGDXL/Mitsubishi+RV-2AJ+limitaties+en+aansturing#MitsubishiRV-2AJlimitatiesenaansturing-Deelvraag5:Metwelkprotocolkanderobotarmaangestuurdworden?
9 1 : static const std::string mb4_prefix = "1;1;";
10 :
11 3 : static const std::string mb4_ctor = mb4_prefix + "RSTALRM\r" + mb4_prefix
12 2 : + "OPEN=usertool\r" + mb4_prefix
13 2 : + "CNTLON\r" + mb4_prefix + "SRVON\r";
14 :
15 2 : static const std::string mb4_dtor =
16 2 : mb4_prefix + "CNTLOFF\r" + mb4_prefix + "SRVOFF\r";
17 :
18 : namespace HLR
19 : {
20 : namespace Kinematics
21 : {
22 : namespace RV2AJ
23 : {
24 0 : Driver::Driver()
25 : {
26 : // bind topics and services
27 0 : pub_mb4 =
28 0 : nh.advertise<std_msgs::String>("serial_request_topic", 1000, true);
29 0 : srv_continue =
30 0 : nh.advertiseService("rv2aj_driver_continue", &Driver::cont, this);
31 0 : srv_pause = nh.advertiseService("rv2aj_driver_pause", &Driver::pause, this);
32 0 : srv_clear = nh.advertiseService("rv2aj_driver_clear", &Driver::clear, this);
33 0 : srv_queue_count = nh.advertiseService(
34 0 : "rv2aj_driver_queue_count", &Driver::queue_count, this);
35 0 : srv_queue_add =
36 0 : nh.advertiseService("rv2aj_driver_queue_add", &Driver::queue_add, this);
37 :
38 : // init arm
39 : // TODO write to serial
40 : // std_msgs::String mb4;
41 : // mb4.data = mb4_ctor;
42 : // pub_mb4.publish(mb4);
43 :
44 : // spawn worker thread
45 0 : worker = std::thread([this] { work(); });
46 0 : }
47 :
48 0 : Driver::~Driver()
49 : {
50 : // stop worker thread
51 0 : worker_state_m.lock();
52 0 : worker_state = State::exit;
53 0 : worker_state_m.unlock();
54 0 : commands_m.lock();
55 0 : commands_cv.notify_all();
56 0 : commands_m.unlock();
57 0 : worker.join();
58 :
59 : // stop arm
60 : // TODO write to serial
61 0 : std_msgs::String mb4;
62 0 : mb4.data = mb4_dtor;
63 0 : pub_mb4.publish(mb4);
64 0 : }
65 :
66 0 : bool Driver::cont(HLR::RV2AJDriverContinue::Request& req[[maybe_unused]],
67 : HLR::RV2AJDriverContinue::Response& res[[maybe_unused]])
68 : {
69 0 : std::lock_guard<std::mutex> l(worker_state_m);
70 0 : switch (worker_state)
71 : {
72 0 : case State::pause:
73 : {
74 0 : worker_state = State::cont;
75 0 : std::lock_guard<std::mutex> l_c(commands_m);
76 0 : commands_cv.notify_one();
77 0 : break;
78 : }
79 :
80 0 : case State::cont:
81 : case State::exit:
82 0 : return false;
83 : }
84 :
85 0 : return true;
86 : }
87 :
88 0 : bool Driver::pause(HLR::RV2AJDriverPause::Request& req[[maybe_unused]],
89 : HLR::RV2AJDriverPause::Response& res[[maybe_unused]])
90 : {
91 0 : std::lock_guard<std::mutex> l(worker_state_m);
92 0 : switch (worker_state)
93 : {
94 0 : case State::cont:
95 0 : worker_state = State::pause;
96 0 : break;
97 :
98 0 : case State::pause:
99 : case State::exit:
100 0 : return false;
101 : }
102 :
103 0 : return true;
104 : }
105 :
106 0 : bool Driver::clear(HLR::RV2AJDriverClear::Request& req[[maybe_unused]],
107 : HLR::RV2AJDriverClear::Response& res[[maybe_unused]])
108 : {
109 0 : std::lock_guard<std::mutex> l(commands_m);
110 0 : std::size_t i = commands.size();
111 0 : while (i > 0)
112 : {
113 0 : commands.pop();
114 0 : --i;
115 : }
116 :
117 0 : return true;
118 : }
119 :
120 0 : bool Driver::queue_count(
121 : HLR::RV2AJDriverQueueCount::Request& req[[maybe_unused]],
122 : HLR::RV2AJDriverQueueCount::Response& res)
123 : {
124 0 : std::lock_guard<std::mutex> l(commands_m);
125 0 : res.count = commands.size();
126 :
127 0 : return true;
128 : }
129 :
130 0 : bool Driver::queue_add(HLR::RV2AJDriverQueueAdd::Request& req,
131 : HLR::RV2AJDriverQueueAdd::Response& res[[maybe_unused]])
132 : {
133 : // speed is a percentage, enforce it
134 : // note that 0% is also not a valid speed, must be > 0
135 0 : if (req.speed <= 0.0 || req.speed > 100.0)
136 : {
137 0 : return false;
138 : }
139 :
140 0 : std::lock_guard<std::mutex> l(commands_m);
141 : // note that J4 and J7 aren't used for RV-2AJ 5DOF
142 0 : commands.push({req.base,
143 0 : req.shoulder,
144 0 : req.elbow,
145 : 0.0,
146 0 : req.wrist,
147 0 : req.wrist_rotate,
148 : 0.0,
149 0 : req.speed});
150 0 : commands_cv.notify_one();
151 :
152 0 : return true;
153 : }
154 :
155 0 : std::string Driver::Command::to_mb4() const
156 : {
157 0 : std::ostringstream ss;
158 :
159 : // set angles
160 : // note that J4 and J7 aren't used for RV-2AJ 5DOF
161 : // they still must be set in the command string though
162 0 : ss << mb4_prefix << "EXECJCOSIROP=(";
163 0 : ss << base << ',';
164 0 : ss << shoulder << ',';
165 0 : ss << elbow << ',';
166 0 : ss << j4 << ',';
167 0 : ss << wrist << ',';
168 0 : ss << wrist_rotate << ',';
169 0 : ss << j7 << ")\r";
170 :
171 : // set speed
172 : // ss << mb4_prefix << "EXECJOVRD " << speed << '\r';
173 :
174 : // exec move
175 : // ss << mb4_prefix << "EXECMOV JCOSIROP\r";
176 :
177 0 : return ss.str();
178 : }
179 :
180 0 : void Driver::work()
181 : {
182 : // worker state
183 : State state;
184 : // lock used for commands and waiting on notifications
185 0 : std::unique_lock<std::mutex> commands_ul(commands_m, std::defer_lock);
186 : // local copy for when popping commands from the queue
187 : Command cmd;
188 : // msg for MB4 topic containing converted MB4 string
189 0 : std_msgs::String mb4;
190 :
191 : // sync state
192 0 : worker_state_m.lock();
193 0 : state = worker_state;
194 0 : worker_state_m.unlock();
195 :
196 0 : commands_ul.lock();
197 0 : while (state != State::exit)
198 : {
199 : // wait for command to be queued or exit when requested
200 0 : commands_cv.wait(commands_ul, [this, &state] {
201 : // sync state
202 0 : worker_state_m.lock();
203 0 : state = worker_state;
204 0 : worker_state_m.unlock();
205 :
206 0 : return state != State::pause
207 0 : && (!commands.empty() || state == State::exit);
208 : });
209 :
210 0 : do
211 : {
212 0 : switch (state)
213 : {
214 0 : case State::cont:
215 0 : cmd = commands.front();
216 0 : commands.pop();
217 0 : commands_ul.unlock();
218 0 : mb4.data = cmd.to_mb4();
219 0 : pub_mb4.publish(mb4);
220 : // TODO write to serial
221 0 : break;
222 :
223 : // pause cannot happen, cover it anyway
224 0 : case State::pause:
225 : case State::exit:
226 0 : commands_ul.unlock();
227 0 : break;
228 : }
229 :
230 : // sync state
231 0 : worker_state_m.lock();
232 0 : state = worker_state;
233 0 : worker_state_m.unlock();
234 :
235 0 : commands_ul.lock();
236 0 : } while (!commands.empty() && state == State::cont);
237 : }
238 0 : commands_ul.unlock();
239 0 : }
240 :
241 : } // namespace RV2AJ
242 : } // namespace Kinematics
243 3 : } // namespace HLR
|
