rtde_writer.cpp
Go to the documentation of this file.
1 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
2 
3 // -- BEGIN LICENSE BLOCK ----------------------------------------------
4 // Copyright 2019 FZI Forschungszentrum Informatik
5 // Created on behalf of Universal Robots A/S
6 //
7 // Licensed under the Apache License, Version 2.0 (the "License");
8 // you may not use this file except in compliance with the License.
9 // You may obtain a copy of the License at
10 //
11 // http://www.apache.org/licenses/LICENSE-2.0
12 //
13 // Unless required by applicable law or agreed to in writing, software
14 // distributed under the License is distributed on an "AS IS" BASIS,
15 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16 // See the License for the specific language governing permissions and
17 // limitations under the License.
18 // -- END LICENSE BLOCK ------------------------------------------------
19 
20 //----------------------------------------------------------------------
27 //----------------------------------------------------------------------
28 
30 
31 namespace urcl
32 {
33 namespace rtde_interface
34 {
35 RTDEWriter::RTDEWriter(comm::URStream<RTDEPackage>* stream, const std::vector<std::string>& recipe)
36  : stream_(stream), recipe_(recipe), queue_{ 32 }, running_(false), package_(recipe_)
37 {
38 }
39 
40 void RTDEWriter::init(uint8_t recipe_id)
41 {
42  recipe_id_ = recipe_id;
44  running_ = true;
45  writer_thread_ = std::thread(&RTDEWriter::run, this);
46 }
47 
49 {
50  uint8_t buffer[4096];
51  size_t size;
52  size_t written;
53  std::unique_ptr<DataPackage> package;
54  while (running_)
55  {
56  if (queue_.waitDequeTimed(package, 1000000))
57  {
58  package->setRecipeID(recipe_id_);
59  size = package->serializePackage(buffer);
60  stream_->write(buffer, size, written);
61  }
62  }
63  URCL_LOG_DEBUG("Write thread ended.");
64 }
65 
66 bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction)
67 {
68  if (speed_slider_fraction > 1.0 || speed_slider_fraction < 0.0)
69  {
70  std::stringstream ss;
71  ss << "Speed slider fraction should be between 0 and 1. The speed slider fraction is "
72  << static_cast<int>(speed_slider_fraction);
73  URCL_LOG_ERROR(ss.str().c_str());
74  return false;
75  }
76 
77  std::lock_guard<std::mutex> guard(package_mutex_);
78  uint32_t mask = 1;
79  bool success = true;
80  success = package_.setData("speed_slider_mask", mask);
81  success = success && package_.setData("speed_slider_fraction", speed_slider_fraction);
82 
83  if (success)
84  {
85  if (!queue_.tryEnqueue(std::unique_ptr<DataPackage>(new DataPackage(package_))))
86  {
87  return false;
88  }
89  }
90  mask = 0;
91  success = package_.setData("speed_slider_mask", mask);
92  return success;
93 }
94 
95 bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value)
96 {
97  if (output_pin > 7)
98  {
99  std::stringstream ss;
100  ss << "Standard digital output pins goes from 0 to 7. The output pin to change is " << static_cast<int>(output_pin);
101  URCL_LOG_ERROR(ss.str().c_str());
102  return false;
103  }
104 
105  std::lock_guard<std::mutex> guard(package_mutex_);
106  uint8_t mask = pinToMask(output_pin);
107  bool success = true;
108  uint8_t digital_output;
109  if (value)
110  {
111  digital_output = 255;
112  }
113  else
114  {
115  digital_output = 0;
116  }
117  success = package_.setData("standard_digital_output_mask", mask);
118  success = success && package_.setData("standard_digital_output", digital_output);
119 
120  if (success)
121  {
122  if (!queue_.tryEnqueue(std::unique_ptr<DataPackage>(new DataPackage(package_))))
123  {
124  return false;
125  }
126  }
127  mask = 0;
128  success = package_.setData("standard_digital_output_mask", mask);
129  return success;
130 }
131 
132 bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value)
133 {
134  if (output_pin > 7)
135  {
136  std::stringstream ss;
137  ss << "Configurable digital output pins goes from 0 to 7. The output pin to change is "
138  << static_cast<int>(output_pin);
139  URCL_LOG_ERROR(ss.str().c_str());
140  return false;
141  }
142 
143  std::lock_guard<std::mutex> guard(package_mutex_);
144  uint8_t mask = pinToMask(output_pin);
145  bool success = true;
146  uint8_t digital_output;
147  if (value)
148  {
149  digital_output = 255;
150  }
151  else
152  {
153  digital_output = 0;
154  }
155  success = package_.setData("configurable_digital_output_mask", mask);
156  success = success && package_.setData("configurable_digital_output", digital_output);
157 
158  if (success)
159  {
160  if (!queue_.tryEnqueue(std::unique_ptr<DataPackage>(new DataPackage(package_))))
161  {
162  return false;
163  }
164  }
165  mask = 0;
166  success = package_.setData("configurable_digital_output_mask", mask);
167  return success;
168 }
169 
170 bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value)
171 {
172  if (output_pin > 1)
173  {
174  std::stringstream ss;
175  ss << "Tool digital output pins goes from 0 to 1. The output pin to change is " << static_cast<int>(output_pin);
176  URCL_LOG_ERROR(ss.str().c_str());
177  return false;
178  }
179 
180  std::lock_guard<std::mutex> guard(package_mutex_);
181  uint8_t mask = pinToMask(output_pin);
182  bool success = true;
183  uint8_t digital_output;
184  if (value)
185  {
186  digital_output = 255;
187  }
188  else
189  {
190  digital_output = 0;
191  }
192  success = package_.setData("tool_digital_output_mask", mask);
193  success = success && package_.setData("tool_digital_output", digital_output);
194 
195  if (success)
196  {
197  if (!queue_.tryEnqueue(std::unique_ptr<DataPackage>(new DataPackage(package_))))
198  {
199  return false;
200  }
201  }
202  mask = 0;
203  success = package_.setData("tool_digital_output_mask", mask);
204  return success;
205 }
206 
207 bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value, const AnalogOutputType type)
208 {
209  if (output_pin > 1)
210  {
211  std::stringstream ss;
212  ss << "Standard analog output goes from 0 to 1. The output pin to change is " << static_cast<int>(output_pin);
213  URCL_LOG_ERROR(ss.str().c_str());
214  return false;
215  }
216  if (value > 1.0 || value < 0.0)
217  {
218  std::stringstream ss;
219  ss << "Analog output value should be between 0 and 1. The value is " << static_cast<double>(value);
220  URCL_LOG_ERROR(ss.str().c_str());
221  return false;
222  }
223 
224  std::lock_guard<std::mutex> guard(package_mutex_);
225  uint8_t mask = pinToMask(output_pin);
226 
227  bool success = true;
228  success = package_.setData("standard_analog_output_mask", mask);
230  {
231  auto output_type_bits = [](const uint8_t pin, const uint8_t type) { return type << pin; };
232  uint8_t output_type = output_type_bits(output_pin, toUnderlying(type));
233  success = success && package_.setData("standard_analog_output_type", output_type);
234  }
235  success = success && package_.setData("standard_analog_output_0", value);
236  success = success && package_.setData("standard_analog_output_1", value);
237 
238  if (success)
239  {
240  if (!queue_.tryEnqueue(std::unique_ptr<DataPackage>(new DataPackage(package_))))
241  {
242  return false;
243  }
244  }
245  mask = 0;
246  success = package_.setData("standard_analog_output_mask", mask);
247  return success;
248 }
249 
250 uint8_t RTDEWriter::pinToMask(uint8_t pin)
251 {
252  if (pin > 7)
253  {
254  return 0;
255  }
256 
257  return 1 << pin;
258 }
259 
260 bool RTDEWriter::sendInputBitRegister(uint32_t register_id, bool value)
261 {
262  if (register_id < 64 || register_id > 127)
263  {
264  std::stringstream ss;
265  ss << "Input bit register goes from 64 to 127. The register id to change is " << static_cast<int>(register_id);
266  URCL_LOG_ERROR(ss.str().c_str());
267  return false;
268  }
269 
270  std::lock_guard<std::mutex> guard(package_mutex_);
271  std::stringstream ss;
272  ss << "input_bit_register_" << register_id;
273 
274  bool success = package_.setData(ss.str(), value);
275 
276  if (success)
277  {
278  if (!queue_.tryEnqueue(std::unique_ptr<DataPackage>(new DataPackage(package_))))
279  {
280  return false;
281  }
282  }
283  return success;
284 }
285 
286 bool RTDEWriter::sendInputIntRegister(uint32_t register_id, int32_t value)
287 {
288  if (register_id < 24 || register_id > 47)
289  {
290  std::stringstream ss;
291  ss << "Input int register goes from 24 to 47. The register id to change is " << static_cast<int>(register_id);
292  URCL_LOG_ERROR(ss.str().c_str());
293  return false;
294  }
295 
296  std::lock_guard<std::mutex> guard(package_mutex_);
297  std::stringstream ss;
298  ss << "input_int_register_" << register_id;
299 
300  bool success = package_.setData(ss.str(), value);
301 
302  if (success)
303  {
304  if (!queue_.tryEnqueue(std::unique_ptr<DataPackage>(new DataPackage(package_))))
305  {
306  return false;
307  }
308  }
309  return success;
310 }
311 
312 bool RTDEWriter::sendInputDoubleRegister(uint32_t register_id, double value)
313 {
314  if (register_id < 24 || register_id > 47)
315  {
316  std::stringstream ss;
317  ss << "Input double register goes from 24 to 47. The register id to change is " << static_cast<int>(register_id);
318  URCL_LOG_ERROR(ss.str().c_str());
319  return false;
320  }
321 
322  std::lock_guard<std::mutex> guard(package_mutex_);
323  std::stringstream ss;
324  ss << "input_double_register_" << register_id;
325 
326  bool success = package_.setData(ss.str(), value);
327 
328  if (success)
329  {
330  if (!queue_.tryEnqueue(std::unique_ptr<DataPackage>(new DataPackage(package_))))
331  {
332  return false;
333  }
334  }
335  return success;
336 }
337 
338 } // namespace rtde_interface
339 } // namespace urcl
urcl::comm::URStream
The stream is an abstraction of the TCPSocket that offers reading a full UR data package out of the s...
Definition: stream.h:40
urcl::rtde_interface::RTDEWriter::pinToMask
uint8_t pinToMask(uint8_t pin)
Definition: rtde_writer.cpp:250
urcl::rtde_interface::RTDEWriter::writer_thread_
std::thread writer_thread_
Definition: rtde_writer.h:167
urcl::rtde_interface::DataPackage
The DataPackage class handles communication in the form of RTDE data packages both to and from the ro...
Definition: data_package.h:60
urcl::rtde_interface::RTDEWriter::sendSpeedSlider
bool sendSpeedSlider(double speed_slider_fraction)
Creates a package to request setting a new value for the speed slider.
Definition: rtde_writer.cpp:66
urcl::rtde_interface::RTDEWriter::RTDEWriter
RTDEWriter()=delete
urcl::rtde_interface::RTDEWriter::queue_
moodycamel::BlockingReaderWriterQueue< std::unique_ptr< DataPackage > > queue_
Definition: rtde_writer.h:166
urcl
Definition: bin_parser.h:36
urcl::rtde_interface::RTDEWriter::sendToolDigitalOutput
bool sendToolDigitalOutput(uint8_t output_pin, bool value)
Creates a package to request setting a new value for one of the tool output pins.
Definition: rtde_writer.cpp:170
URCL_LOG_ERROR
#define URCL_LOG_ERROR(...)
Definition: log.h:26
urcl::rtde_interface::RTDEWriter::sendInputIntRegister
bool sendInputIntRegister(uint32_t register_id, int32_t value)
Creates a package to request setting a new value for an input_int_register.
Definition: rtde_writer.cpp:286
URCL_LOG_DEBUG
#define URCL_LOG_DEBUG(...)
Definition: log.h:23
urcl::rtde_interface::DataPackage::setData
bool setData(const std::string &name, T &val)
Set a data field in the DataPackage.
Definition: data_package.h:178
urcl::rtde_interface::RTDEWriter::sendInputDoubleRegister
bool sendInputDoubleRegister(uint32_t register_id, double value)
Creates a package to request setting a new value for an input_double_register.
Definition: rtde_writer.cpp:312
urcl::rtde_interface::RTDEWriter::package_
DataPackage package_
Definition: rtde_writer.h:169
urcl::rtde_interface::RTDEWriter::sendConfigurableDigitalOutput
bool sendConfigurableDigitalOutput(uint8_t output_pin, bool value)
Creates a package to request setting a new value for one of the configurable digital output pins.
Definition: rtde_writer.cpp:132
urcl::rtde_interface::RTDEWriter::run
void run()
The writer thread loop, continually serializing and sending packages to the robot.
Definition: rtde_writer.cpp:48
rtde_writer.h
urcl::rtde_interface::RTDEWriter::running_
bool running_
Definition: rtde_writer.h:168
urcl::rtde_interface::RTDEWriter::package_mutex_
std::mutex package_mutex_
Definition: rtde_writer.h:170
urcl::rtde_interface::RTDEWriter::sendStandardDigitalOutput
bool sendStandardDigitalOutput(uint8_t output_pin, bool value)
Creates a package to request setting a new value for one of the standard digital output pins.
Definition: rtde_writer.cpp:95
urcl::rtde_interface::RTDEWriter::init
void init(uint8_t recipe_id)
Starts the writer thread, which periodically clears the queue to write packages to the robot.
Definition: rtde_writer.cpp:40
urcl::rtde_interface::RTDEWriter::stream_
comm::URStream< RTDEPackage > * stream_
Definition: rtde_writer.h:163
urcl::AnalogOutputType::SET_ON_TEACH_PENDANT
@ SET_ON_TEACH_PENDANT
urcl::toUnderlying
constexpr std::underlying_type< E >::type toUnderlying(const E e) noexcept
Converts an enum type to its underlying type.
Definition: types.h:80
urcl::rtde_interface::RTDEWriter::recipe_id_
uint8_t recipe_id_
Definition: rtde_writer.h:165
urcl::rtde_interface::RTDEWriter::sendStandardAnalogOutput
bool sendStandardAnalogOutput(uint8_t output_pin, double value, const AnalogOutputType type=AnalogOutputType::SET_ON_TEACH_PENDANT)
Creates a package to request setting a new value for one of the standard analog output pins.
Definition: rtde_writer.cpp:207
urcl::rtde_interface::RTDEWriter::sendInputBitRegister
bool sendInputBitRegister(uint32_t register_id, bool value)
Creates a package to request setting a new value for an input_bit_register.
Definition: rtde_writer.cpp:260
urcl::rtde_interface::DataPackage::initEmpty
void initEmpty()
Initializes to contained list with empty values based on the recipe.
Definition: data_package.cpp:570
urcl::AnalogOutputType
AnalogOutputType
Definition: datatypes.h:83


ur_client_library
Author(s): Thomas Timm Andersen, Simon Rasmussen, Felix Exner, Lea Steffen, Tristan Schnell
autogenerated on Mon May 26 2025 02:35:58