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 0 : std_msgs::String mb4;
41 0 : mb4.data = mb4_ctor;
42 :
43 0 : ROS_INFO("BEFORE PUBLISH");
44 0 : pub_mb4.publish(mb4);
45 0 : ROS_INFO("AFTER PUBLISH");
46 :
47 : // spawn worker thread
48 0 : worker = std::thread([this] { work(); });
49 0 : }
50 :
51 0 : Driver::~Driver()
52 : {
53 : // stop worker thread
54 0 : worker_state_m.lock();
55 0 : worker_state = State::exit;
56 0 : worker_state_m.unlock();
57 0 : commands_m.lock();
58 0 : commands_cv.notify_all();
59 0 : commands_m.unlock();
60 0 : worker.join();
61 :
62 : // stop arm
63 : // TODO write to serial
64 0 : std_msgs::String mb4;
65 0 : mb4.data = mb4_dtor;
66 0 : pub_mb4.publish(mb4);
67 0 : }
68 :
69 0 : bool Driver::cont(HLR::RV2AJDriverContinue::Request& req[[maybe_unused]],
70 : HLR::RV2AJDriverContinue::Response& res[[maybe_unused]])
71 : {
72 0 : std::lock_guard<std::mutex> l(worker_state_m);
73 0 : switch (worker_state)
74 : {
75 0 : case State::pause:
76 : {
77 0 : worker_state = State::cont;
78 0 : std::lock_guard<std::mutex> l_c(commands_m);
79 0 : commands_cv.notify_one();
80 0 : break;
81 : }
82 :
83 0 : case State::cont:
84 : case State::exit:
85 0 : return false;
86 : }
87 :
88 0 : return true;
89 : }
90 :
91 0 : bool Driver::pause(HLR::RV2AJDriverPause::Request& req[[maybe_unused]],
92 : HLR::RV2AJDriverPause::Response& res[[maybe_unused]])
93 : {
94 0 : std::lock_guard<std::mutex> l(worker_state_m);
95 0 : switch (worker_state)
96 : {
97 0 : case State::cont:
98 0 : worker_state = State::pause;
99 0 : break;
100 :
101 0 : case State::pause:
102 : case State::exit:
103 0 : return false;
104 : }
105 :
106 0 : return true;
107 : }
108 :
109 0 : bool Driver::clear(HLR::RV2AJDriverClear::Request& req[[maybe_unused]],
110 : HLR::RV2AJDriverClear::Response& res[[maybe_unused]])
111 : {
112 0 : std::lock_guard<std::mutex> l(commands_m);
113 0 : std::size_t i = commands.size();
114 0 : while (i > 0)
115 : {
116 0 : commands.pop();
117 0 : --i;
118 : }
119 :
120 0 : return true;
121 : }
122 :
123 0 : bool Driver::queue_count(
124 : HLR::RV2AJDriverQueueCount::Request& req[[maybe_unused]],
125 : HLR::RV2AJDriverQueueCount::Response& res)
126 : {
127 0 : std::lock_guard<std::mutex> l(commands_m);
128 0 : res.count = commands.size();
129 :
130 0 : return true;
131 : }
132 :
133 0 : bool Driver::queue_add(HLR::RV2AJDriverQueueAdd::Request& req,
134 : HLR::RV2AJDriverQueueAdd::Response& res[[maybe_unused]])
135 : {
136 : // speed is a percentage, enforce it
137 : // note that 0% is also not a valid speed, must be > 0
138 0 : if (req.speed <= 0.0 || req.speed > 100.0)
139 : {
140 0 : return false;
141 : }
142 :
143 0 : std::lock_guard<std::mutex> l(commands_m);
144 : // note that J4 and J7 aren't used for RV-2AJ 5DOF
145 0 : commands.push({req.base,
146 0 : req.shoulder,
147 0 : req.elbow,
148 : 0.0,
149 0 : req.wrist,
150 0 : req.wrist_rotate,
151 : 0.0,
152 0 : req.speed});
153 0 : commands_cv.notify_one();
154 :
155 0 : return true;
156 : }
157 :
158 0 : std::string Driver::Command::to_mb4() const
159 : {
160 0 : std::ostringstream ss;
161 :
162 : // set angles
163 : // note that J4 and J7 aren't used for RV-2AJ 5DOF
164 : // they still must be set in the command string though
165 0 : ss << mb4_prefix << "EXECJCOSIROP=(";
166 0 : ss << base << ',';
167 0 : ss << shoulder << ',';
168 0 : ss << elbow << ',';
169 0 : ss << j4 << ',';
170 0 : ss << wrist << ',';
171 0 : ss << wrist_rotate << ',';
172 0 : ss << j7 << ")\r";
173 :
174 : // set speed
175 0 : ss << mb4_prefix << "EXECJOVRD " << speed << '\r';
176 :
177 : // exec move
178 0 : ss << mb4_prefix << "EXECMOV JCOSIROP\r";
179 :
180 0 : return ss.str();
181 : }
182 :
183 0 : void Driver::work()
184 : {
185 : // worker state
186 : State state;
187 : // lock used for commands and waiting on notifications
188 0 : std::unique_lock<std::mutex> commands_ul(commands_m, std::defer_lock);
189 : // local copy for when popping commands from the queue
190 : Command cmd;
191 : // msg for MB4 topic containing converted MB4 string
192 0 : std_msgs::String mb4;
193 :
194 : // sync state
195 0 : worker_state_m.lock();
196 0 : state = worker_state;
197 0 : worker_state_m.unlock();
198 :
199 0 : commands_ul.lock();
200 0 : while (state != State::exit)
201 : {
202 : // wait for command to be queued or exit when requested
203 0 : commands_cv.wait(commands_ul, [this, &state] {
204 : // sync state
205 0 : worker_state_m.lock();
206 0 : state = worker_state;
207 0 : worker_state_m.unlock();
208 :
209 0 : return state != State::pause
210 0 : && (!commands.empty() || state == State::exit);
211 : });
212 :
213 0 : do
214 : {
215 0 : switch (state)
216 : {
217 0 : case State::cont:
218 0 : cmd = commands.front();
219 0 : commands.pop();
220 0 : commands_ul.unlock();
221 0 : mb4.data = cmd.to_mb4();
222 0 : pub_mb4.publish(mb4);
223 : // TODO write to serial
224 0 : break;
225 :
226 : // pause cannot happen, cover it anyway
227 0 : case State::pause:
228 : case State::exit:
229 0 : commands_ul.unlock();
230 0 : break;
231 : }
232 :
233 : // sync state
234 0 : worker_state_m.lock();
235 0 : state = worker_state;
236 0 : worker_state_m.unlock();
237 :
238 0 : commands_ul.lock();
239 0 : } while (!commands.empty() && state == State::cont);
240 : }
241 0 : commands_ul.unlock();
242 0 : }
243 :
244 : } // namespace RV2AJ
245 : } // namespace Kinematics
246 3 : } // namespace HLR
|
