RobotUnitModuleDevices.cpp
Go to the documentation of this file.
1/*
2 * This file is part of ArmarX.
3 *
4 * ArmarX is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * @package RobotAPI::ArmarXObjects::RobotUnit
17 * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
18 * @date 2018
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22
23#include "RobotUnitModuleDevices.h"
24
25#include <ArmarXCore/core/util/StringHelpers.h>
26#include <ArmarXCore/core/util/algorithm.h>
27
28#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
29
30#include "RobotUnitModuleControlThreadDataBuffer.h"
31#include "RobotUnitModuleRobotData.h"
32
33namespace armarx::RobotUnitModule
34{
35 const std::string Devices::rtThreadTimingsSensorDeviceName = "RTThreadTimings";
36
37 ControlDeviceDescription
38 Devices::getControlDeviceDescription(const std::string& name, const Ice::Current&) const
39 {
40 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
41 throwIfDevicesNotReady(__FUNCTION__);
42 std::lock_guard<MutexType> guard{controlDevicesMutex};
43 if (!controlDevices.has(name))
44 {
45 std::stringstream ss;
46 ss << "getControlDeviceDescription: There is no ControlDevice '" << name
47 << "'. There are these ControlDevices: " << controlDevices.keys();
48 throw InvalidArgumentException{ss.str()};
49 }
50 return getControlDeviceDescription(controlDevices.index(name));
51 }
52
53 ControlDeviceDescriptionSeq
54 Devices::getControlDeviceDescriptions(const Ice::Current&) const
55 {
56 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
57 if (!areDevicesReady())
58 {
59 return {};
60 }
61 std::lock_guard<MutexType> guard{controlDevicesMutex};
62 ControlDeviceDescriptionSeq r;
63 r.reserve(getNumberOfControlDevices());
64 for (auto idx : getIndices(controlDevices.values()))
65 {
66 r.emplace_back(getControlDeviceDescription(idx));
67 }
68 throwIfDevicesNotReady(__FUNCTION__);
69 return r;
70 }
71
72 std::size_t
73 Devices::getNumberOfControlDevices() const
74 {
75 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
76 throwIfDevicesNotReady(__FUNCTION__);
77 return controlDevices.size();
78 }
79
80 std::size_t
81 Devices::getNumberOfSensorDevices() const
82 {
83 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
84 throwIfDevicesNotReady(__FUNCTION__);
85 return sensorDevices.size();
86 }
87
88 std::size_t
89 Devices::getSensorDeviceIndex(const std::string& deviceName) const
90 {
91 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
92 throwIfDevicesNotReady(__FUNCTION__);
93 std::lock_guard<MutexType> guard{sensorDevicesMutex};
94 ARMARX_CHECK_EXPRESSION(sensorDevices.has(deviceName));
95 return sensorDevices.index(deviceName);
96 }
97
98 std::size_t
99 Devices::getControlDeviceIndex(const std::string& deviceName) const
100 {
101 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
102 throwIfDevicesNotReady(__FUNCTION__);
103 std::lock_guard<MutexType> guard{controlDevicesMutex};
104 ARMARX_CHECK_EXPRESSION(controlDevices.has(deviceName));
105 return controlDevices.index(deviceName);
106 }
107
108 ConstSensorDevicePtr
109 Devices::getSensorDevice(const std::string& sensorDeviceName) const
110 {
111 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
112 throwIfDevicesNotReady(__FUNCTION__);
113 std::lock_guard<MutexType> guard{sensorDevicesMutex};
114 return sensorDevices.at(sensorDeviceName, SensorDevice::NullPtr);
115 }
116
117 ConstControlDevicePtr
118 Devices::getControlDevice(const std::string& deviceName) const
119 {
120 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
121 throwIfDevicesNotReady(__FUNCTION__);
122 std::lock_guard<MutexType> guard{controlDevicesMutex};
123 return controlDevices.at(deviceName, ControlDevice::NullPtr);
124 }
125
126 Ice::StringSeq
127 Devices::getControlDeviceNames(const Ice::Current&) const
128 {
129 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
130 throwIfDevicesNotReady(__FUNCTION__);
131 std::lock_guard<MutexType> guard{controlDevicesMutex};
132 return controlDevices.keys();
133 }
134
135 ControlDeviceDescription
136 Devices::getControlDeviceDescription(std::size_t idx) const
137 {
138 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
139 throwIfDevicesNotReady(__FUNCTION__);
140 std::lock_guard<MutexType> guard{controlDevicesMutex};
141 const ControlDevicePtr& controlDevice = controlDevices.at(idx);
142 ControlDeviceDescription data;
143 data.deviceName = controlDevice->getDeviceName();
144 data.tags.assign(controlDevice->getTags().begin(), controlDevice->getTags().end());
145 for (const auto& jointCtrl : controlDevice->getJointControllers())
146 {
147 data.contolModeToTargetType[jointCtrl->getControlMode()].targetType =
148 jointCtrl->getControlTarget()->getControlTargetType();
149 data.contolModeToTargetType[jointCtrl->getControlMode()].hardwareControlMode =
150 jointCtrl->getHardwareControlMode();
151 }
152 throwIfDevicesNotReady(__FUNCTION__);
153 return data;
154 }
155
156 ControlDeviceStatus
157 Devices::getControlDeviceStatus(std::size_t idx) const
158 {
159 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
160 throwIfDevicesNotReady(__FUNCTION__);
161 std::lock_guard<MutexType> guard{controlDevicesMutex};
162 const ControlDevicePtr& controlDevice = controlDevices.at(idx);
163 ControlDeviceStatus status;
164 const auto activeJointCtrl =
165 _module<ControlThreadDataBuffer>().getActivatedJointControllers().at(idx);
166 status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode()
167 : std::string{"!!JointController is nullptr!!"};
168 status.deviceName = controlDevice->getDeviceName();
169 const auto requestedJointControllers =
170 _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
171 ARMARX_CHECK_EXPRESSION(requestedJointControllers.at(idx));
172 status.requestedControlMode = requestedJointControllers.at(idx)->getControlMode();
173 for (const auto& targ :
174 _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().control.at(idx))
175 {
176 status.controlTargetValues[targ->getControlMode()] =
177 targ->toVariants(_module<ControlThreadDataBuffer>()
178 .getSensorAndControlBuffer()
179 .sensorValuesTimestamp);
180 }
181 status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
182 throwIfDevicesNotReady(__FUNCTION__);
183 return status;
184 }
185
186 ControlDeviceStatus
187 Devices::getControlDeviceStatus(const std::string& name, const Ice::Current&) const
188 {
189 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
190 throwIfDevicesNotReady(__FUNCTION__);
191 std::lock_guard<MutexType> guard{controlDevicesMutex};
192 if (!controlDevices.has(name))
193 {
194 std::stringstream ss;
195 ss << "getControlDeviceStatus: There is no ControlDevice '" << name
196 << "'. There are these ControlDevices: " << controlDevices.keys();
197 throw InvalidArgumentException{ss.str()};
198 }
199 return getControlDeviceStatus(controlDevices.index(name));
200 }
201
202 ControlDeviceStatusSeq
203 Devices::getControlDeviceStatuses(const Ice::Current&) const
204 {
205 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
206 if (!areDevicesReady())
207 {
208 return {};
209 }
210 std::lock_guard<MutexType> guard{controlDevicesMutex};
211 ControlDeviceStatusSeq r;
212 r.reserve(getNumberOfControlDevices());
213 for (auto idx : getIndices(controlDevices.values()))
214 {
215 r.emplace_back(getControlDeviceStatus(idx));
216 }
217 throwIfDevicesNotReady(__FUNCTION__);
218 return r;
219 }
220
221 Ice::StringSeq
222 Devices::getSensorDeviceNames(const Ice::Current&) const
223 {
224 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
225 throwIfDevicesNotReady(__FUNCTION__);
226 std::lock_guard<MutexType> guard{sensorDevicesMutex};
227 return sensorDevices.keys();
228 }
229
230 SensorDeviceDescription
231 Devices::getSensorDeviceDescription(std::size_t idx) const
232 {
233 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
234 throwIfDevicesNotReady(__FUNCTION__);
235 std::lock_guard<MutexType> guard{sensorDevicesMutex};
236 const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
237 SensorDeviceDescription data;
238 data.deviceName = sensorDevice->getDeviceName();
239 data.tags.assign(sensorDevice->getTags().begin(), sensorDevice->getTags().end());
240 data.sensorValueType = sensorDevice->getSensorValueType();
241 throwIfDevicesNotReady(__FUNCTION__);
242 return data;
243 }
244
245 SensorDeviceStatus
246 Devices::getSensorDeviceStatus(std::size_t idx) const
247 {
248 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
249 throwIfDevicesNotReady(__FUNCTION__);
250 std::lock_guard<MutexType> guard{sensorDevicesMutex};
251 const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
252 SensorDeviceStatus status;
253 status.deviceName = sensorDevice->getDeviceName();
254 status.sensorValue = _module<ControlThreadDataBuffer>()
255 .getSensorAndControlBuffer()
256 .sensors.at(idx)
257 ->toVariants(_module<ControlThreadDataBuffer>()
258 .getSensorAndControlBuffer()
259 .sensorValuesTimestamp);
260 status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
261 throwIfDevicesNotReady(__FUNCTION__);
262 return status;
263 }
264
265 SensorDeviceDescription
266 Devices::getSensorDeviceDescription(const std::string& name, const Ice::Current&) const
267 {
268 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
269 throwIfDevicesNotReady(__FUNCTION__);
270 std::lock_guard<MutexType> guard{sensorDevicesMutex};
271 if (!sensorDevices.has(name))
272 {
273 std::stringstream ss;
274 ss << "getSensorDeviceDescription: There is no SensorDevice '" << name
275 << "'. There are these SensorDevices: " << sensorDevices.keys();
276 throw InvalidArgumentException{ss.str()};
277 }
278 return getSensorDeviceDescription(sensorDevices.index(name));
279 }
280
281 SensorDeviceDescriptionSeq
282 Devices::getSensorDeviceDescriptions(const Ice::Current&) const
283 {
284 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
285 std::lock_guard<MutexType> guard{sensorDevicesMutex};
286 if (!areDevicesReady())
287 {
288 return {};
289 }
290 SensorDeviceDescriptionSeq r;
291 r.reserve(getNumberOfSensorDevices());
292 for (auto idx : getIndices(sensorDevices.values()))
293 {
294 r.emplace_back(getSensorDeviceDescription(idx));
295 }
296 return r;
297 }
298
299 SensorDeviceStatus
300 Devices::getSensorDeviceStatus(const std::string& name, const Ice::Current&) const
301 {
302 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
303 throwIfDevicesNotReady(__FUNCTION__);
304 std::lock_guard<MutexType> guard{sensorDevicesMutex};
305 if (!sensorDevices.has(name))
306 {
307 std::stringstream ss;
308 ss << "getSensorDeviceStatus: There is no SensorDevice '" << name
309 << "'. There are these SensorDevices: " << sensorDevices.keys();
310 throw InvalidArgumentException{ss.str()};
311 }
312 return getSensorDeviceStatus(sensorDevices.index(name));
313 }
314
315 SensorDeviceStatusSeq
316 Devices::getSensorDeviceStatuses(const Ice::Current&) const
317 {
318 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
319 if (!areDevicesReady())
320 {
321 return {};
322 }
323 std::lock_guard<MutexType> guard{sensorDevicesMutex};
324 SensorDeviceStatusSeq r;
325 r.reserve(getNumberOfSensorDevices());
326 for (auto idx : getIndices(sensorDevices.values()))
327 {
328 r.emplace_back(getSensorDeviceStatus(idx));
329 }
330 throwIfDevicesNotReady(__FUNCTION__);
331 return r;
332 }
333
334 void
335 Devices::addControlDevice(const ControlDevicePtr& cd)
336 {
337 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
338 ARMARX_DEBUG << "ControlDevice " << &cd;
339 throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
340 {
341 std::lock_guard<MutexType> guard{controlDevicesMutex};
342 //check it
343 if (!cd)
344 {
345 std::stringstream ss;
346 ss << "armarx::RobotUnit::addControlDevice: ControlDevice is nullptr";
347 ARMARX_ERROR << ss.str();
348 throw InvalidArgumentException{ss.str()};
349 }
350 if (!cd->getJointEmergencyStopController())
351 {
352 std::stringstream ss;
353 ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
354 << " has null JointEmergencyStopController (this is not allowed)";
355 ARMARX_ERROR << ss.str();
356 throw InvalidArgumentException{ss.str()};
357 }
358 if (!cd->getJointStopMovementController())
359 {
360 std::stringstream ss;
361 ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
362 << " has null getJointStopMovementController (this is not allowed)";
363 ARMARX_ERROR << ss.str();
364 throw InvalidArgumentException{ss.str()};
365 }
366 //add it
367 ARMARX_DEBUG << "Adding the ControlDevice " << cd->getDeviceName() << " " << &cd;
368 controlDevices.add(cd->getDeviceName(), cd);
369 cd->owner = this;
370 ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
371 }
372 ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
373 throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
374 }
375
376 void
377 Devices::addSensorDevice(const SensorDevicePtr& sd)
378 {
379 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
380 ARMARX_DEBUG << "SensorDevice " << &sd;
381 throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
382 {
383 std::lock_guard<MutexType> guard{sensorDevicesMutex};
384 //check it
385 if (!sd)
386 {
387 std::stringstream ss;
388 ss << "armarx::RobotUnit::addSensorDevice: SensorDevice is nullptr";
389 ARMARX_ERROR << ss.str();
390 throw InvalidArgumentException{ss.str()};
391 }
392 if (!sd->getSensorValue())
393 {
394 std::stringstream ss;
395 ss << "armarx::RobotUnit::addSensorDevice: SensorDevice " << sd->getDeviceName()
396 << " has null SensorValue (this is not allowed)";
397 ARMARX_ERROR << ss.str();
398 throw InvalidArgumentException{ss.str()};
399 }
400 //add it
401 if (sd->getDeviceName() == rtThreadTimingsSensorDeviceName)
402 {
403 ARMARX_DEBUG << "Device is the " << rtThreadTimingsSensorDeviceName;
404 if (!std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd))
405 {
406 throw InvalidArgumentException{
407 "You tried to add a SensorDevice with the name " + sd->getDeviceName() +
408 " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"};
409 }
410 //this checks if we already added such a device (do this before setting timingSensorDevice)
411 ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd;
412 sensorDevices.add(sd->getDeviceName(), sd);
413 sd->owner = this;
414 rtThreadTimingsSensorDevice =
415 std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
416 }
417 else if (sd->getDeviceName() == GlobalRobotLocalizationSensorDevice::DeviceName())
418 {
419 ARMARX_DEBUG << "Device is the " << sd->getDeviceName();
420 if (!std::dynamic_pointer_cast<GlobalRobotLocalizationSensorDevice>(sd))
421 {
422 throw InvalidArgumentException{
423 "You tried to add a SensorDevice with the name " + sd->getDeviceName() +
424 " which does not derive from GlobalRobotLocalizationSensorDevice. (Don't "
425 "do this)"};
426 }
427 //this checks if we already added such a device (do this before setting timingSensorDevice)
428 ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd;
429 sensorDevices.add(sd->getDeviceName(), sd);
430 sd->owner = this;
431 globalRobotLocalizationSensorDevice =
432 sd; //std::dynamic_pointer_cast<GlobalRobotLocalizationSensorDevice>(sd);
433 }
434 else
435 {
436 ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd;
437 sensorDevices.add(sd->getDeviceName(), sd);
438 sd->owner = this;
439 }
440 }
441 ARMARX_INFO << "added SensorDevice " << sd->getDeviceName()
442 << " (valuetype = " << sd->getSensorValueType() << ")";
443 throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
444 }
445
446 RTThreadTimingsSensorDevicePtr
447 Devices::createRTThreadTimingSensorDevice() const
448 {
449 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
450 return std::make_shared<RTThreadTimingsSensorDeviceImpl<>>(rtThreadTimingsSensorDeviceName);
451 }
452
453 void
454 Devices::_postFinishRunning()
455 {
456 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
457 std::lock_guard<MutexType> guardS{sensorDevicesMutex};
458 std::lock_guard<MutexType> guardC{controlDevicesMutex};
459 controlDevicesConstPtr.clear();
460 sensorDevicesConstPtr.clear();
461 sensorDevices.clear();
462 controlDevices.clear();
463 }
464
465 std::vector<JointController*>
466 Devices::getStopMovementJointControllers() const
467 {
468 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
469 throwIfDevicesNotReady(__FUNCTION__);
470 std::lock_guard<MutexType> guard{controlDevicesMutex};
471 std::vector<JointController*> controllers;
472 controllers.reserve(controlDevices.values().size());
473 for (const ControlDevicePtr& dev : controlDevices.values())
474 {
475 ARMARX_CHECK_NOT_NULL(dev);
476 controllers.emplace_back(dev->rtGetJointStopMovementController());
477 ARMARX_CHECK_NOT_NULL(controllers.back());
478 }
479 ARMARX_CHECK_EQUAL(controlDevices.size(), controllers.size());
480 throwIfDevicesNotReady(__FUNCTION__);
481 return controllers;
482 }
483
484 std::vector<JointController*>
485 Devices::getEmergencyStopJointControllers() const
486 {
487 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
488 throwIfDevicesNotReady(__FUNCTION__);
489 std::lock_guard<MutexType> guard{controlDevicesMutex};
490 std::vector<JointController*> controllers;
491 controllers.reserve(controlDevices.values().size());
492 for (const ControlDevicePtr& dev : controlDevices.values())
493 {
494 ARMARX_CHECK_NOT_NULL(dev);
495 controllers.emplace_back(dev->rtGetJointEmergencyStopController());
496 ARMARX_CHECK_NOT_NULL(controllers.back());
497 }
498 ARMARX_CHECK_EQUAL(controlDevices.size(), controllers.size());
499 throwIfDevicesNotReady(__FUNCTION__);
500 return controllers;
501 }
502
503 void
504 Devices::_preFinishDeviceInitialization()
505 {
506 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
507 std::lock_guard<MutexType> guardS{sensorDevicesMutex};
508 std::lock_guard<MutexType> guardC{controlDevicesMutex};
509 if (!sensorDevices.has(rtThreadTimingsSensorDeviceName))
510 {
511 addSensorDevice(createRTThreadTimingSensorDevice());
512 }
513
514 // this device will be used by the PlatformUnit to make the robot's global pose
515 // available to e.g. the NJointControllers.
516 if (getProperty<bool>("GlobalPoseCorrectionAndLocalizationSensorEnabled").getValue())
517 {
518 addSensorDevice(std::make_shared<GlobalRobotPoseCorrectionSensorDevice>());
519 addSensorDevice(std::make_shared<GlobalRobotLocalizationSensorDevice>());
520 }
521 }
522
523 void
524 Devices::_postFinishDeviceInitialization()
525 {
526 ARMARX_TRACE;
527 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
528 std::lock_guard<MutexType> guardS{sensorDevicesMutex};
529 std::lock_guard<MutexType> guardC{controlDevicesMutex};
530 ARMARX_DEBUG << "checking " << controlDevices.size() << " ControlDevices:";
531 {
532 ARMARX_TRACE;
533 for (const ControlDevicePtr& controlDevice : controlDevices.values())
534 {
535 ARMARX_CHECK_EXPRESSION(controlDevice);
536 ARMARX_DEBUG << "----" << controlDevice->getDeviceName();
537 if (!controlDevice->hasJointController(ControlModes::EmergencyStop))
538 {
539 std::stringstream s;
540 s << "ControlDevice " << controlDevice->getDeviceName()
541 << " has no JointController with ControlMode " << ControlModes::EmergencyStop
542 << " (to fix this, add a JointController with this ControlMode to the "
543 "ControlDevice).\nAvailable controllers: "
544 << controlDevice->getControlModes();
545 ARMARX_ERROR << "--------" << s.str();
546 throw LogicError{s.str()};
547 }
548 if (!controlDevice->hasJointController(ControlModes::StopMovement))
549 {
550 std::stringstream s;
551 s << "ControlDevice " << controlDevice->getDeviceName()
552 << " has no JointController with ControlMode \"" << ControlModes::StopMovement
553 << "\" (to fix this, add a JointController with this ControlMode to the "
554 "ControlDevice) \nAvailable controllers: "
555 << controlDevice->getControlModes();
556 ARMARX_ERROR << "--------" << s.str();
557 throw LogicError{s.str()};
558 }
559 }
560 }
561 ARMARX_DEBUG << "checking " << controlDevices.size() << " SensorDevices:";
562 {
563 ARMARX_TRACE;
564 for (const SensorDevicePtr& sensorDevice : sensorDevices.values())
565 {
566 ARMARX_CHECK_EXPRESSION(sensorDevice);
567 ARMARX_DEBUG << "----" << sensorDevice->getDeviceName();
568 if (!sensorDevice->getSensorValue())
569 {
570 std::stringstream s;
571 s << "SensorDevice " << sensorDevice->getSensorValue()
572 << " has null SensorValue";
573 ARMARX_ERROR << "--------" << s.str();
574 throw LogicError{s.str()};
575 }
576 ARMARX_CHECK_EXPRESSION(sensorDevice);
577 }
578 }
579 ARMARX_DEBUG << "copying device ptrs to const ptr map";
580 {
581 ARMARX_TRACE;
582 for (const auto& dev : controlDevices.values())
583 {
584 controlDevicesConstPtr[dev->getDeviceName()] = dev;
585 }
586 for (const auto& dev : sensorDevices.values())
587 {
588 sensorDevicesConstPtr[dev->getDeviceName()] = dev;
589 }
590 }
591 ARMARX_DEBUG << "copy sensor values";
592 {
593 ARMARX_TRACE;
594 ARMARX_CHECK_EXPRESSION(sensorValues.empty());
595 sensorValues.reserve(sensorDevices.values().size());
596 for (const SensorDevicePtr& dev : sensorDevices.values())
597 {
598 ARMARX_CHECK_NOT_NULL(dev);
599 sensorValues.emplace_back(dev->getSensorValue());
600 ARMARX_CHECK_NOT_NULL(sensorValues.back());
601 }
602 }
603 ARMARX_DEBUG << "copy control targets";
604 {
605 ARMARX_TRACE;
606 ARMARX_CHECK_EXPRESSION(controlTargets.empty());
607 controlTargets.reserve(controlDevices.values().size());
608 for (const ControlDevicePtr& dev : controlDevices.values())
609 {
610 ARMARX_CHECK_NOT_NULL(dev);
611 controlTargets.emplace_back();
612 controlTargets.back().reserve(dev->rtGetJointControllers().size());
613 for (JointController* ctrl : dev->rtGetJointControllers())
614 {
615 ARMARX_CHECK_NOT_NULL(ctrl);
616 controlTargets.back().emplace_back(ctrl->getControlTarget());
617 ARMARX_CHECK_NOT_NULL(controlTargets.back().back().get());
618 }
619 }
620 }
621 ARMARX_DEBUG << "setup ControlDeviceHardwareControlModeGroups";
622 {
623 ARMARX_TRACE;
624 ctrlModeGroups.groupIndices.assign(getNumberOfControlDevices(), IndexSentinel());
625
626 if (!ctrlModeGroups.groupsMerged.empty())
627 {
628 ARMARX_DEBUG << "Remove control devs from ControlDeviceHardwareControlModeGroups ("
629 << ctrlModeGroups.groups.size() << ")";
630 const auto groupsMerged = ctrlModeGroups.groupsMerged;
631 for (const auto& dev : groupsMerged)
632 {
633 if (controlDevices.has(dev))
634 {
635 continue;
636 }
637 ctrlModeGroups.groupsMerged.erase(dev);
638 for (auto& group : ctrlModeGroups.groups)
639 {
640 group.erase(dev);
641 }
642 ARMARX_DEBUG << "----removing nonexistent device: " << dev;
643 }
644 //remove empty groups
645 std::vector<std::set<std::string>> cleanedGroups;
646 cleanedGroups.reserve(ctrlModeGroups.groups.size());
647 for (auto& group : ctrlModeGroups.groups)
648 {
649 const auto sz = group.size();
650 if (sz == 1)
651 {
652 ARMARX_DEBUG << "----removing group with one dev: " << *group.begin();
653 }
654 else if (sz > 1)
655 {
656 cleanedGroups.emplace_back(std::move(group));
657 }
658 }
659 ctrlModeGroups.groups = cleanedGroups;
660 ARMARX_DEBUG << "----number of groups left: " << ctrlModeGroups.groups.size();
661 }
662 if (!ctrlModeGroups.groupsMerged.empty())
663 {
664 ARMARX_DEBUG
665 << "Checking control modes for ControlDeviceHardwareControlModeGroups ("
666 << ctrlModeGroups.groups.size() << ")";
667 ctrlModeGroups.deviceIndices.resize(ctrlModeGroups.groups.size());
668 //iterate over groups
669 for (std::size_t groupIdx = 0; groupIdx < ctrlModeGroups.groups.size(); ++groupIdx)
670 {
671 const std::set<std::string>& group = ctrlModeGroups.groups.at(groupIdx);
672 ctrlModeGroups.deviceIndices.at(groupIdx).reserve(group.size());
673 ARMARX_CHECK_EXPRESSION(!group.empty());
674 ARMARX_DEBUG << "----Group " << groupIdx << " size: " << group.size();
675 //gets a map of ControlMode->HardwareControlMode for the given device
676 const auto getControlModeToHWControlMode = [&](const std::string& devname)
677 {
678 std::map<std::string, std::string> controlModeToHWControlMode;
679 const ControlDevicePtr& cd = controlDevices.at(devname);
680 for (const auto& jointCtrl : cd->getJointControllers())
681 {
682 controlModeToHWControlMode[jointCtrl->getControlMode()] =
683 jointCtrl->getHardwareControlMode();
684 }
685 return controlModeToHWControlMode;
686 };
687 //get modes of first dev
688 const auto controlModeToHWControlMode =
689 getControlModeToHWControlMode(*group.begin());
690 //check other devs
691 for (const auto& devname : group)
692 {
693 ARMARX_CHECK_EXPRESSION(controlDevices.has(devname))
694 << "The ControlDeviceHardwareControlModeGroups property contains "
695 "device names not existent in the robot: "
696 << devname << "\navailable:\n"
697 << controlDevices.keys();
698 //Assert all devices in a group have the same control modes with the same hw controle modes
699 const auto controlModeToHWControlModeForDevice =
700 getControlModeToHWControlMode(devname);
701 ARMARX_CHECK_EXPRESSION(controlModeToHWControlModeForDevice ==
702 controlModeToHWControlMode)
703 << "Error for control modes of device '" << devname << "'\n"
704 << "it has the modes: " << controlModeToHWControlModeForDevice
705 << "\n but should have the modes: " << controlModeToHWControlMode;
706 //insert the device index into the device indices
707 const auto devIdx = controlDevices.index(devname);
708 ctrlModeGroups.deviceIndices.at(groupIdx).emplace_back(devIdx);
709 //insert the group index into the group indices (+ check the current group index is the sentinel
710 ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.size() > devIdx);
711 ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.at(devIdx) ==
712 IndexSentinel());
713 ctrlModeGroups.groupIndices.at(devIdx) = groupIdx;
714 ARMARX_DEBUG << "------- " << devname;
715 }
716 }
717 }
718 }
719 ARMARX_DEBUG << "create mapping from sensor values to robot nodes";
720 {
721 ARMARX_TRACE;
722 ARMARX_CHECK_EXPRESSION(simoxRobotSensorValueMapping.empty());
723 VirtualRobot::RobotPtr r = _module<RobotData>().cloneRobot();
724 const auto nodes = r->getRobotNodes();
725 for (std::size_t idxRobot = 0; idxRobot < nodes.size(); ++idxRobot)
726 {
727 const VirtualRobot::RobotNodePtr& node = nodes.at(idxRobot);
728 if (node->isJoint())
729 {
730 const auto& name = node->getName();
731 if (sensorDevices.has(name))
732 {
733 const auto& dev = sensorDevices.at(name);
734 if (dev->getSensorValue()->isA<SensorValue1DoFActuatorPosition>())
735 {
736 SimoxRobotSensorValueMapping m;
737 m.idxRobot = idxRobot;
738 m.idxSens = sensorDevices.index(name);
739 simoxRobotSensorValueMapping.emplace_back(m);
740 }
741 else
742 {
743 ARMARX_WARNING
744 << "SensorValue for SensorDevice " << name << " is of type "
745 << dev->getSensorValueType()
746 << " which does not derive SensorValue1DoFActuatorPosition";
747 }
748 }
749 else
750 {
751 ARMARX_INFO << "No SensorDevice for RobotNode: " << name;
752 }
753 }
754 }
755 }
756
757 ARMARX_VERBOSE << "ControlDevices:\n" << controlDevices.keys();
758 ARMARX_VERBOSE << "SensorDevices:\n" << sensorDevices.keys();
759 }
760
761 void
762 Devices::_preOnInitRobotUnit()
763 {
764 throwIfInControlThread(BOOST_CURRENT_FUNCTION);
765 //ControlDeviceHardwareControlModeGroups
766 const std::string controlDeviceHardwareControlModeGroupsStr =
767 getProperty<std::string>("ControlDevices_HardwareControlModeGroups").getValue();
768 if (!controlDeviceHardwareControlModeGroupsStr.empty())
769 {
770 const auto numGroups = std::count(controlDeviceHardwareControlModeGroupsStr.begin(),
771 controlDeviceHardwareControlModeGroupsStr.end(),
772 ';') +
773 1;
774 ctrlModeGroups.groups.reserve(numGroups);
775 std::vector<std::string> strGroups =
776 Split(controlDeviceHardwareControlModeGroupsStr, ";");
777 for (const auto& gstr : strGroups)
778 {
779 bool trimDeviceNames = true;
780 std::vector<std::string> strElems = Split(gstr, ",", trimDeviceNames);
781 std::set<std::string> group;
782 for (auto& device : strElems)
783 {
784 ARMARX_CHECK_EXPRESSION(!device.empty())
785 << "The ControlDeviceHardwareControlModeGroups property contains empty "
786 "device names";
787 ARMARX_CHECK_EXPRESSION(!ctrlModeGroups.groupsMerged.count(device))
788 << "The ControlDeviceHardwareControlModeGroups property contains duplicate "
789 "device names: "
790 << device;
791 ctrlModeGroups.groupsMerged.emplace(device);
792 group.emplace(std::move(device));
793 }
794 if (!group.empty())
795 {
796 ARMARX_DEBUG << "adding device group:\n" << ARMARX_STREAM_PRINTER
797 {
798 for (const auto& elem : group)
799 {
800 out << " " << elem << "\n";
801 }
802 };
803 ctrlModeGroups.groups.emplace_back(std::move(group));
804 }
805 }
806 }
807 }
808} // namespace armarx::RobotUnitModule
ARMARX_STREAM_PRINTER
#define ARMARX_STREAM_PRINTER
use this macro to write output code that is executed when printed and thus not executed if the debug ...
Definition Logging.h:310
armarx::Component::getProperty
Property< PropertyType > getProperty(const std::string &name)
Definition PropertyUser.h:180
armarx::ControlDevice::NullPtr
static const ControlDevicePtr NullPtr
A static const nullptr in case a const ref to a nullptr needs to be returned.
Definition ControlDevice.h:67
armarx::PropertyUser::getProperty
Property< PropertyType > getProperty(const std::string &name)
Property creation and retrieval.
Definition PropertyUser.h:180
armarx::RobotUnitModule::Devices::getSensorDeviceNames
Ice::StringSeq getSensorDeviceNames(const Ice::Current &=Ice::emptyCurrent) const override
Returns the names of all SensorDevices for the robot.
Definition RobotUnitModuleDevices.cpp:222
armarx::RobotUnitModule::Devices::getControlDeviceNames
Ice::StringSeq getControlDeviceNames(const Ice::Current &=Ice::emptyCurrent) const override
Returns the names of all ControlDevices for the robot.
Definition RobotUnitModuleDevices.cpp:127
armarx::RobotUnitModule::Devices::getControlDeviceDescription
ControlDeviceDescription getControlDeviceDescription(const std::string &name, const Ice::Current &=Ice::emptyCurrent) const override
Return the ControlDeviceDescription for the given ControlDevice.
Definition RobotUnitModuleDevices.cpp:38
armarx::RobotUnitModule::Devices::getSensorDeviceDescription
SensorDeviceDescription getSensorDeviceDescription(const std::string &name, const Ice::Current &=Ice::emptyCurrent) const override
Return the SensorDeviceDescription for the given SensorDevice.
Definition RobotUnitModuleDevices.cpp:266
armarx::RobotUnitModule::Devices::addSensorDevice
void addSensorDevice(const SensorDevicePtr &sd)
Adds a SensorDevice to the robot.
Definition RobotUnitModuleDevices.cpp:377
armarx::RobotUnitModule::Devices::addControlDevice
void addControlDevice(const ControlDevicePtr &cd)
Adds a ControlDevice to the robot.
Definition RobotUnitModuleDevices.cpp:335
armarx::RobotUnitModule::Devices::getNumberOfControlDevices
std::size_t getNumberOfControlDevices() const
Returns the number of ControlDevices.
Definition RobotUnitModuleDevices.cpp:73
armarx::RobotUnitModule::Devices::getSensorDeviceDescriptions
SensorDeviceDescriptionSeq getSensorDeviceDescriptions(const Ice::Current &=Ice::emptyCurrent) const override
Return the SensorDeviceDescriptions for all SensorDevices.
Definition RobotUnitModuleDevices.cpp:282
armarx::RobotUnitModule::Devices::getControlDeviceStatuses
ControlDeviceStatusSeq getControlDeviceStatuses(const Ice::Current &=Ice::emptyCurrent) const override
Return the ControlDeviceStatuses for all ControlDevices.
Definition RobotUnitModuleDevices.cpp:203
armarx::RobotUnitModule::Devices::getSensorDeviceIndex
std::size_t getSensorDeviceIndex(const std::string &deviceName) const
Returns the SensorDevice's index.
Definition RobotUnitModuleDevices.cpp:89
armarx::RobotUnitModule::Devices::getSensorDeviceStatuses
SensorDeviceStatusSeq getSensorDeviceStatuses(const Ice::Current &=Ice::emptyCurrent) const override
Return the SensorDeviceStatuses for all SensorDevices.
Definition RobotUnitModuleDevices.cpp:316
armarx::RobotUnitModule::Devices::getSensorDevice
ConstSensorDevicePtr getSensorDevice(const std::string &deviceName) const
TODO move to attorney for NJointControllerBase.
Definition RobotUnitModuleDevices.cpp:109
armarx::RobotUnitModule::Devices::getControlDevice
ConstControlDevicePtr getControlDevice(const std::string &deviceName) const
Returns the ControlDevice.
Definition RobotUnitModuleDevices.cpp:118
armarx::RobotUnitModule::Devices::createRTThreadTimingSensorDevice
virtual RTThreadTimingsSensorDevicePtr createRTThreadTimingSensorDevice() const
Creates the SensorDevice used to log timings in the ControlThread (This function is supposed to be us...
Definition RobotUnitModuleDevices.cpp:447
armarx::RobotUnitModule::Devices::getControlDeviceIndex
std::size_t getControlDeviceIndex(const std::string &deviceName) const
Returns the ControlDevice's index.
Definition RobotUnitModuleDevices.cpp:99
armarx::RobotUnitModule::Devices::getControlDeviceStatus
ControlDeviceStatus getControlDeviceStatus(const std::string &name, const Ice::Current &=Ice::emptyCurrent) const override
Return the ControlDeviceStatus for the given ControlDevice.
Definition RobotUnitModuleDevices.cpp:187
armarx::RobotUnitModule::Devices::getControlDeviceDescriptions
ControlDeviceDescriptionSeq getControlDeviceDescriptions(const Ice::Current &=Ice::emptyCurrent) const override
Return the ControlDeviceDescriptions for all ControlDevices.
Definition RobotUnitModuleDevices.cpp:54
armarx::RobotUnitModule::Devices::getSensorDeviceStatus
SensorDeviceStatus getSensorDeviceStatus(const std::string &name, const Ice::Current &=Ice::emptyCurrent) const override
Return the SensorDeviceStatus for the given SensorDevice.
Definition RobotUnitModuleDevices.cpp:300
armarx::RobotUnitModule::Devices::getNumberOfSensorDevices
std::size_t getNumberOfSensorDevices() const
Returns the number of SensorDevices.
Definition RobotUnitModuleDevices.cpp:81
armarx::RobotUnitModule::ModuleBase::_module
T & _module()
Returns this as ref to the given type.
Definition RobotUnitModuleBase.h:249
armarx::RobotUnitModule::ModuleBase::areDevicesReady
bool areDevicesReady() const
Returns whether Devices are ready.
Definition RobotUnitModuleBase.cpp:405
armarx::RobotUnitModule::ModuleBase::throwIfInControlThread
void throwIfInControlThread(const std::string &fnc) const
Throws if the current thread is the ControlThread.
Definition RobotUnitModuleBase.cpp:418
armarx::RobotUnitModule::ModuleBase::throwIfStateIsNot
void throwIfStateIsNot(const std::set< RobotUnitState > &stateSet, const std::string &fnc, bool onlyWarn=false) const
Throws an exception if the current state is not in.
Definition RobotUnitModuleBase.cpp:458
armarx::RobotUnitModule::ModuleBase::IndexSentinel
static constexpr std::size_t IndexSentinel()
Returns a sentinel value for an index (std::numeric_limits<std::size_t>::max())
Definition RobotUnitModuleBase.h:720
armarx::RobotUnitModule::ModuleBase::throwIfDevicesNotReady
void throwIfDevicesNotReady(const std::string &fnc) const
Throws if the Devices are not ready.
Definition RobotUnitModuleBase.cpp:532
armarx::SensorDevice::NullPtr
static const SensorDevicePtr NullPtr
A static const nullptr in case a const ref to a nullptr needs to be returned.
Definition SensorDevice.h:63
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition TimeUtil.cpp:42
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition ExpressionException.h:73
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition ExpressionException.h:206
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition ExpressionException.h:130
ARMARX_INFO
#define ARMARX_INFO
The normal logging level.
Definition Logging.h:181
ARMARX_ERROR
#define ARMARX_ERROR
The logging level for unexpected behaviour, that must be fixed.
Definition Logging.h:196
ARMARX_DEBUG
#define ARMARX_DEBUG
The logging level for output that is only interesting while debugging.
Definition Logging.h:184
ARMARX_WARNING
#define ARMARX_WARNING
The logging level for unexpected behaviour, but not a serious problem.
Definition Logging.h:193
ARMARX_VERBOSE
#define ARMARX_VERBOSE
The logging level for verbose information.
Definition Logging.h:187
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition Bus.h:19
armarx::control::common::getValue
T getValue(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName)
Definition utils.h:80
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition CtrlUtil.h:33
armarx::getIndices
std::vector< KeyT > getIndices(const std::map< KeyT, ValT > &c)
Definition algorithm.h:307
armarx::Split
std::vector< std::string > Split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition StringHelpers.cpp:29
armarx::ConstControlDevicePtr
std::shared_ptr< const class ControlDevice > ConstControlDevicePtr
Definition NJointControllerBase.h:77
armarx::ConstSensorDevicePtr
std::shared_ptr< const class SensorDevice > ConstSensorDevicePtr
Definition NJointControllerBase.h:78
control
This file is part of ArmarX.
ARMARX_TRACE
#define ARMARX_TRACE
Definition trace.h:77