HokuyoLaserUnit.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::HokuyoLaserUnit
17  * @author Fabian Paus ( fabian dot paus at kit dot edu )
18  * @date 2017
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "HokuyoLaserUnit.h"
24 
25 #include <SimoxUtility/algorithm/string/string_tools.h>
26 
32 
33 #include <HokuyoLaserScannerDriver/urg_utils.h>
34 
35 using namespace armarx;
36 
38 {
39  addPlugin(heartbeat);
40 }
41 
42 
43 void
45 {
46  offeringTopicFromProperty("RobotHealthTopicName");
47  offeringTopicFromProperty("DebugObserverName");
48 
49  laserScannerListenerProxyName = getProperty<std::string>("LaserScannerListenerName").getValue();
50  usingProxyFromProperty("LaserScannerListenerName");
51  ARMARX_INFO << "Going to report to listener " << laserScannerListenerProxyName;
52  updatePeriod = getProperty<int>("UpdatePeriod").getValue();
53  angleOffset = getProperty<float>("AngleOffset").getValue();
54 
55  std::string deviceStrings = getProperty<std::string>("Devices").getValue();
56  std::vector<std::string> splitDeviceStrings = Split(deviceStrings, ";");
57  devices.clear();
58  devices.reserve(splitDeviceStrings.size());
59  for (std::string const& deviceString : splitDeviceStrings)
60  {
61  std::vector<std::string> deviceInfo = Split(deviceString, ",");
62  if (deviceInfo.size() != 3)
63  {
64  ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
65  << " (split size: " << deviceInfo.size() << ")";
66  continue;
67  }
68 
69  try
70  {
71  int port = std::stoi(deviceInfo[1]);
72 
73  HokuyoLaserScanDevice& device = devices.emplace_back();
74  device.ip = deviceInfo[0];
75  device.port = port;
76  device.frame = deviceInfo[2];
77  device.angleOffset = angleOffset;
78  device.connected = false;
79 
80  device.componentName = getName();
81  }
82  catch (std::exception const& ex)
83  {
84  ARMARX_WARNING << "Could not convert port to integer for laser scanner device "
85  << deviceString << " (port is " << deviceInfo[1] << ") : " << ex.what();
86  continue;
87  }
88  }
89 }
90 
91 void
93 {
95 
96  getProxyFromProperty(listenerPrx, "LaserScannerListenerName");
97  debugObserver = getTopic<DebugObserverInterfacePrx>(
98  getProperty<std::string>("DebugObserverName").getValue());
99 
100  connectedDevices.clear();
101  for (HokuyoLaserScanDevice& device : devices)
102  {
103  if (device.task)
104  {
105  device.task->stop();
106  device.task = nullptr;
107  }
108 
109  // connecting devices for first time
110  if (!device.reconnect())
111  {
112  ARMARX_WARNING << "Not starting task for laser scanner with IP: " << device.ip
113  << ", Port: " << device.port;
114  continue;
115  }
116 
117  ARMARX_CHECK_NOT_NULL(heartbeat);
118  heartbeat->signUp(device.ip,
121  {"LaserScanner", "Localization"},
122  "HokuyoLaserScanDevice");
123 
124  LaserScannerInfo info;
125  info.device = device.ip;
126  info.frame = device.frame;
127  int minStep = 0, maxStep = 0;
128  urg_step_min_max(&device.urg, &minStep, &maxStep);
129  info.minAngle = urg_step2rad(&device.urg, minStep);
130  info.maxAngle = urg_step2rad(&device.urg, maxStep);
131 
132 
133  int lengthDataSize = urg_max_data_size(&device.urg);
134  info.stepSize = (info.maxAngle - info.minAngle) / (maxStep - minStep);
135  device.lengthData.resize(lengthDataSize);
136 
137  device.listenerPrx = listenerPrx;
138  device.robotHealthPlugin = heartbeat;
139  device.debugObserver = debugObserver;
140 
141  connectedDevices.push_back(info);
142 
143  ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", " << info.minAngle
144  << ", " << info.maxAngle << ", " << info.stepSize;
145 
146  device.task = new RunningTask<HokuyoLaserScanDevice>(
147  &device, &HokuyoLaserScanDevice::run, "HokuyoLaserScanUpdate_" + device.ip);
148  device.task->start();
149  }
150 }
151 
152 void
154 {
155  for (HokuyoLaserScanDevice& device : devices)
156  {
157  if (device.task)
158  {
159  device.task->stop();
160  device.task = nullptr;
161  }
162  if (device.connected)
163  {
164  urg_close(&device.urg);
165  device.connected = false;
166  }
167  }
168 }
169 
170 void
172 {
173 }
174 
177 {
180 }
181 
182 std::string
183 HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
184 {
185  return laserScannerListenerProxyName;
186 }
187 
188 LaserScannerInfoSeq
189 HokuyoLaserUnit::getConnectedDevices(const Ice::Current&) const
190 {
191  return connectedDevices;
192 }
193 
194 bool
196 {
197  if (connected)
198  {
199  ARMARX_INFO << "Disconnecting from laser scanner with IP " << ip;
200  urg_close(&urg);
201  connected = false;
202  }
203  ARMARX_INFO << "Reconnecting to " << ip << ":" << port;
204  int ret = urg_open(&urg, URG_ETHERNET, ip.c_str(), port);
205  connected = (ret == 0);
206  if (!connected)
207  {
208  ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: " << ip
209  << ", Port: " << port << ", Error: " << ret << ")";
210  return false;
211  }
212  ret = urg_start_measurement(&urg, URG_DISTANCE, URG_SCAN_INFINITY, 0);
213  connected = (ret == 0);
214  if (connected)
215  {
216  ARMARX_IMPORTANT << "Reconnected succesffully to " << ip << ":" << port;
217  return true;
218  }
219  else
220  {
222  << "Could not start measurement for laser scanner device using URG driver (IP: " << ip
223  << ", Port: " << port << ", Error: " << ret << ")";
224  return false;
225  }
226 }
227 
228 void
230 {
231  ARMARX_TRACE;
232  while (!task->isStopped())
233  {
234  ARMARX_INFO << deactivateSpam() << "Running update loop for laserscanner device " << ip;
235 
236  IceUtil::Time time_start = TimeUtil::GetTime();
237 
238  if (errorCounter > 10)
239  {
240  ARMARX_ERROR << "Device " << ip << " has too many consecutive errors!";
241  // assume dead
242  reconnect();
243  }
244  if (connected)
245  {
246  int lengthDataSize = urg_get_distance(&urg, lengthData.data(), nullptr);
247  if (lengthDataSize < 0)
248  {
250  << "Could not get measurement for laser scanner (IP: " << ip
251  << ", Port: " << port << ", Error: " << lengthDataSize << ")";
252  errorCounter++;
253  continue;
254  }
255  IceUtil::Time time_measure = TimeUtil::GetTime();
256  TimestampVariantPtr now(new TimestampVariant(time_measure));
257 
258  scan.clear();
259  scan.reserve(lengthDataSize);
260  for (int stepIndex = 0; stepIndex < lengthDataSize; ++stepIndex)
261  {
262  LaserScanStep step;
263  long distance = lengthData[stepIndex];
264  // TODO: Extract the min and max valid value for distance into parameters?
265  if (distance >= 21 && distance <= 30000)
266  {
267  step.angle =
268  angleOffset + (float)urg_index2rad(&urg, stepIndex); // Convert steps to rad
269  step.distance = (float)distance; // Data is already in mm
270  scan.push_back(step);
271  }
272  }
273 
274  IceUtil::Time time_update = TimeUtil::GetTime();
275 
276  errorCounter = 0;
277 
278  if (listenerPrx)
279  {
280 
281  listenerPrx->reportSensorValues(ip, frame, scan, now);
282  }
283  else
284  {
285  ARMARX_WARNING << "No proxy available: IP: " << ip << ", Port: " << port;
286  }
287  IceUtil::Time time_proxyReport = TimeUtil::GetTime();
288 
289  if (robotHealthPlugin != nullptr)
290  {
292  }
293  else
294  {
295  ARMARX_WARNING << "No robot health topic available: IP: " << ip
296  << ", Port: " << port;
297  }
298 
299  IceUtil::Time time_topicHeartbeat = TimeUtil::GetTime();
300 
301  IceUtil::Time duration = time_topicHeartbeat - time_start;
302 
303  StringVariantBaseMap durations;
304  durations["total_ms"] = new Variant(duration.toMilliSecondsDouble());
305  durations["measure_ms"] =
306  new Variant((time_measure - time_start).toMilliSecondsDouble());
307  durations["update_ms"] =
308  new Variant((time_update - time_measure).toMilliSecondsDouble());
309  durations["proxy_report_ms"] =
310  new Variant((time_proxyReport - time_update).toMilliSecondsDouble());
311  durations["topic_health_ms"] =
312  new Variant((time_topicHeartbeat - time_proxyReport).toMilliSecondsDouble());
313  debugObserver->setDebugChannel(
314  "LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations);
315 
316  if (duration.toSecondsDouble() > 0.1)
317  {
318  ARMARX_WARNING << "Laserscanner reports are slow"
319  << "Total time: " << duration.toMilliSecondsDouble() << "ms\n"
320  << "Measure: "
321  << (time_measure - time_start).toMilliSecondsDouble() << "ms \n"
322  << "Update: "
323  << (time_update - time_measure).toMilliSecondsDouble() << "ms\n"
324  << "TopicSensor: "
325  << (time_proxyReport - time_update).toMilliSecondsDouble() << "ms\n"
326  << "TopicHeart: "
327  << (time_topicHeartbeat - time_proxyReport).toMilliSecondsDouble()
328  << "ms\n";
329  }
330  }
331  }
332 }
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
armarx::HokuyoLaserUnit::HokuyoLaserUnit
HokuyoLaserUnit()
Definition: HokuyoLaserUnit.cpp:37
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:21
armarx::plugins::HeartbeatComponentPlugin::signUp
void signUp(const std::string &channelName="", const std::vector< std::string > &aliases={}, const std::string &description="")
register component to heartbeat
Definition: HeartbeatComponentPlugin.cpp:14
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
armarx::HokuyoLaserUnit::onInitComponent
void onInitComponent() override
Definition: HokuyoLaserUnit.cpp:44
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::HokuyoLaserScanDevice::task
RunningTask< HokuyoLaserScanDevice >::pointer_type task
Definition: HokuyoLaserUnit.h:86
DateTime.h
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::HokuyoLaserScanDevice::errorCounter
int errorCounter
Definition: HokuyoLaserUnit.h:79
armarx::Split
std::vector< std::string > Split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelperTemplates.h:35
armarx::TimestampVariant
Definition: TimestampVariant.h:54
armarx::Component::offeringTopicFromProperty
void offeringTopicFromProperty(const std::string &propertyName)
Offer a topic whose name is specified by the given property.
Definition: Component.cpp:154
Duration.h
armarx::Component::usingProxyFromProperty
bool usingProxyFromProperty(const std::string &propertyName, const std::string &endpoints="")
Use a proxy whose name is specified by the given property.
Definition: Component.cpp:167
ice_conversions.h
armarx::HokuyoLaserUnit::onDisconnectComponent
void onDisconnectComponent() override
Definition: HokuyoLaserUnit.cpp:153
armarx::ManagedIceObject::addPlugin
PluginT * addPlugin(const std::string prefix="", ParamsT &&...params)
Definition: ManagedIceObject.h:182
armarx::HokuyoLaserScanDevice::port
int port
Definition: HokuyoLaserUnit.h:73
HokuyoLaserUnit.h
armarx::RunningTask
Definition: ArmarXMultipleObjectsScheduler.h:35
armarx::HokuyoLaserUnitPropertyDefinitions
Definition: HokuyoLaserUnit.h:43
Clock.h
IceInternal::Handle< TimestampVariant >
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
armarx::HokuyoLaserScanDevice::frame
std::string frame
Definition: HokuyoLaserUnit.h:74
armarx::Component::getProxyFromProperty
ProxyType getProxyFromProperty(const std::string &propertyName, bool addToDependencies=false, const std::string &endpoints="", bool throwOnProxyError=true)
Get a proxy whose name is specified by the given property.
Definition: Component.h:236
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::HokuyoLaserUnit::onExitComponent
void onExitComponent() override
Definition: HokuyoLaserUnit.cpp:171
armarx::HokuyoLaserScanDevice::run
void run()
Definition: HokuyoLaserUnit.cpp:229
armarx::HokuyoLaserScanDevice::reconnect
bool reconnect()
Definition: HokuyoLaserUnit.cpp:195
TimestampVariant.h
armarx::HokuyoLaserUnit::onConnectComponent
void onConnectComponent() override
Definition: HokuyoLaserUnit.cpp:92
armarx::HokuyoLaserScanDevice
Definition: HokuyoLaserUnit.h:70
armarx::HokuyoLaserScanDevice::componentName
std::string componentName
Definition: HokuyoLaserUnit.h:89
armarx::HokuyoLaserUnit::getConnectedDevices
LaserScannerInfoSeq getConnectedDevices(const Ice::Current &c) const override
Definition: HokuyoLaserUnit.cpp:189
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::HokuyoLaserScanDevice::urg
urg_t urg
Definition: HokuyoLaserUnit.h:77
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
armarx::HokuyoLaserScanDevice::debugObserver
DebugObserverInterfacePrx debugObserver
Definition: HokuyoLaserUnit.h:92
armarx::HokuyoLaserUnit::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: HokuyoLaserUnit.cpp:176
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::HokuyoLaserUnit::getReportTopicName
std::string getReportTopicName(const Ice::Current &c) const override
Definition: HokuyoLaserUnit.cpp:183
armarx::plugins::HeartbeatComponentPlugin::heartbeatOnChannel
void heartbeatOnChannel(const std::string &channelName)
Sends out a heartbeat for a subchannel.
Definition: HeartbeatComponentPlugin.cpp:95
armarx::HokuyoLaserScanDevice::angleOffset
float angleOffset
Definition: HokuyoLaserUnit.h:75
armarx::HokuyoLaserScanDevice::robotHealthPlugin
armarx::plugins::HeartbeatComponentPlugin * robotHealthPlugin
Definition: HokuyoLaserUnit.h:91
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:88
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:107
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
armarx::HokuyoLaserScanDevice::lengthData
std::vector< long > lengthData
Definition: HokuyoLaserUnit.h:78
armarx::HokuyoLaserScanDevice::connected
bool connected
Definition: HokuyoLaserUnit.h:76
armarx::HokuyoLaserScanDevice::ip
std::string ip
Definition: HokuyoLaserUnit.h:72
armarx::HokuyoLaserScanDevice::listenerPrx
LaserScannerUnitListenerPrx listenerPrx
Definition: HokuyoLaserUnit.h:90
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::core::time::Duration::MilliSeconds
static Duration MilliSeconds(std::int64_t milliSeconds)
Constructs a duration in milliseconds.
Definition: Duration.cpp:55
armarx::HokuyoLaserScanDevice::scan
LaserScan scan
Definition: HokuyoLaserUnit.h:87