sick_ldmrs_node.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015, DFKI GmbH
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of DFKI GmbH nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Created on: 20.11.2015
30  *
31  * Authors:
32  * Martin Günther <martin.guenther@dfki.de>
33  * Jochen Sprickerhof <jochen@sprickerhof.de>
34  *
35  */
36 
37 #include <iostream>
38 
40 
42 
43 #include <sick_ldmrs/datatypes/EvalCaseResults.hpp>
44 #include <sick_ldmrs/datatypes/EvalCases.hpp>
45 #include <sick_ldmrs/datatypes/Fields.hpp>
46 #include <sick_ldmrs/datatypes/Measurement.hpp>
47 #include <sick_ldmrs/datatypes/Msg.hpp>
48 #include <sick_ldmrs/datatypes/Scan.hpp>
49 
50 #include <sick_ldmrs/devices/LD_MRS.hpp>
51 
52 #include <sick_ldmrs/tools/errorhandler.hpp>
53 #include <sick_ldmrs/tools/toolbox.hpp>
54 
55 #include <sick_ldmrs_msgs/ObjectArray.h>
56 #include <tf/transform_datatypes.h>
57 
58 
59 namespace sick_ldmrs_driver
60 {
61 
63  : application::BasicApplication()
64  , diagnostics_(diagnostics)
65  , manager_(manager)
66  , expected_frequency_(12.5)
67  , initialized_(false)
68 {
69  dynamic_reconfigure::Server<SickLDMRSDriverConfig>::CallbackType f;
70  f = boost::bind(&SickLDMRS::update_config, this, _1, _2);
71  dynamic_reconfigure_server_.setCallback(f);
72 
73  // point cloud publisher
74  pub_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud", 100);
75 
76  object_pub_ = nh_.advertise<sick_ldmrs_msgs::ObjectArray>("objects", 1);
77 
78  diagnostics_->setHardwareID("none"); // set from device after connection
79  diagnostics_->add("device info", this, &SickLDMRS::produce_diagnostics);
81  // frequency should be target +- 10%
83  // timestamp delta can be from -1 seconds to 1.3x what it ideally is at the lowest frequency
84  diagnostic_updater::TimeStampStatusParam(-1, 1.3 * 1.0 / 12.5));
85 }
86 
88 {
89  delete diagnosticPub_;
90 }
91 
93 {
94  if (isUpsideDown())
95  {
96  ROS_ERROR("Error: upside down mode is active, please disable!");
97  }
98  initialized_ = true;
100 }
101 
103 {
104  devices::LDMRS* ldmrs;
105  ldmrs = dynamic_cast<devices::LDMRS*>(manager_->getFirstDeviceByType(Sourcetype_LDMRS));
106  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Device information.");
107 
108  // REP-138 values (http://www.ros.org/reps/rep-0138.html#diagnostic-keys)
109  stat.add("IP Address", ldmrs->getIpAddress());
110  stat.add("IP Port", 12002); // LUX port number
111  stat.add("Vendor Name", "SICK");
112  stat.add("Product Name", "LD-MRS");
113  stat.add("Firmware Version", ldmrs->getFirmwareVersion()); // includes date, e.g. "3.03.5 2015-01-14 13:32"
114  stat.add("Device ID", ldmrs->getSerialNumber());
115 }
116 
117 void SickLDMRS::setData(BasicData &data)
118 {
119  std::string datatypeStr;
120  std::string sourceIdStr;
121 
122  switch (data.getDatatype())
123  {
124  case Datatype_Scan:
125  datatypeStr = "Scan (" + ::toString(((Scan&)data).getNumPoints()) + " points)";
126  {
127  Scan* scan = dynamic_cast<Scan*>(&data);
128  std::vector<ScannerInfo> scannerInfos = scan->getScannerInfos();
129  if (scannerInfos.size() != 1)
130  {
131  ROS_ERROR("Expected exactly 1 scannerInfo, got %zu!", scannerInfos.size());
132  return;
133  }
134 
135  const Time& time = scannerInfos[0].getStartTimestamp();
136  ROS_DEBUG("setData(): Scan start time: %s (%s)",
137  time.toString().c_str(),
138  time.toLongString().c_str());
139 
140  PointCloud::Ptr cloud = boost::make_shared<PointCloud>();
141  cloud->header.frame_id = config_.frame_id;
142  // not using time stamp from scanner here, because it is delayed by up to 1.5 seconds
143  cloud->header.stamp = (ros::Time::now().toSec() - 1 / expected_frequency_) * 1e6;
144 
145  cloud->height = 1;
146  cloud->width = scan->size();
147  for (size_t i = 0; i < scan->size(); ++i)
148  {
149  const ScanPoint& p = (*scan)[i];
151  np.x = p.getX();
152  np.y = p.getY();
153  np.z = p.getZ();
154  np.echowidth = p.getEchoWidth();
155  np.layer = p.getLayer() + (scannerInfos[0].isRearMirrorSide() ? 4 : 0);
156  np.echo = p.getEchoNum();
157  np.flags = p.getFlags();
158  cloud->points.push_back(np);
159  }
160 
161  sensor_msgs::PointCloud2 msg;
162  pcl::toROSMsg(*cloud, msg);
163  diagnosticPub_->publish(msg);
164  }
165  break;
166  case Datatype_Objects:
167  datatypeStr = "Objects (" + ::toString(((ObjectList&)data).size()) + " objects)";
168  pubObjects((ObjectList&)data);
169  break;
170  case Datatype_Fields:
171  datatypeStr = "Fields (" + ::toString(((Fields&)data).getFields().size()) + " fields, " +
172  ::toString(((Fields&)data).getNumberOfValidFields()) + " of which are valid)";
173  break;
174  case Datatype_EvalCases:
175  datatypeStr = "EvalCases (" + ::toString(((EvalCases&)data).getEvalCases().size()) + " cases)";
176  break;
177  case Datatype_EvalCaseResults:
178  datatypeStr = "EvalCaseResults (" + ::toString(((EvalCaseResults&)data).size()) + " case results)";
179  break;
180  case Datatype_Msg:
181  datatypeStr = "Msg (" + ((Msg&)data).toString() + ")";
182  diagnostics_->broadcast(diagnostic_msgs::DiagnosticStatus::WARN, ((Msg&)data).toString());
183  diagnostics_->force_update();
184  break;
185  case Datatype_MeasurementList:
186  datatypeStr = "MeasurementList (" + ::toString(((MeasurementList&)data).m_list.size()) + " entries)";
187  break;
188  default:
189  datatypeStr = "(unknown)";
190  }
191 
192  sourceIdStr = ::toString(data.getSourceId());
193 
194  ROS_DEBUG("setData(): Called with data of type %s from ID %s", datatypeStr.c_str(), sourceIdStr.c_str());
195 }
196 
197 void SickLDMRS::validate_config(SickLDMRSDriverConfig &conf)
198 {
199  if (conf.start_angle <= conf.end_angle)
200  {
201  ROS_WARN("Start angle must be greater than end angle. Adjusting start_angle.");
202  conf.start_angle = conf.end_angle; // TODO: - 2 * ticks2rad
203  }
204 
205  if (conf.angular_resolution_type != SickLDMRSDriver_ConstantRes
206  && conf.scan_frequency != SickLDMRSDriver_ScanFreq1250)
207  {
208  ROS_WARN("Focused/flexible resolution only available at 12.5 Hz scan frequency. Adjusting scan frequency.");
209  conf.scan_frequency = SickLDMRSDriver_ScanFreq1250;
210  }
211 
212  if (conf.ignore_near_range && conf.layer_range_reduction != SickLDMRSDriver_RangeLowerReduced)
213  {
214  ROS_WARN("If ignore_near_range is set, layer_range_reduction must be set to 'Lower 4 layers reduced range'. Adjusting layer_range_reduction.");
215  conf.layer_range_reduction = SickLDMRSDriver_RangeLowerReduced;
216  }
217 
218  validate_flexres_resolution(conf.flexres_resolution1);
219  validate_flexres_resolution(conf.flexres_resolution2);
220  validate_flexres_resolution(conf.flexres_resolution3);
221  validate_flexres_resolution(conf.flexres_resolution4);
222  validate_flexres_resolution(conf.flexres_resolution5);
223  validate_flexres_resolution(conf.flexres_resolution6);
224  validate_flexres_resolution(conf.flexres_resolution7);
225  validate_flexres_resolution(conf.flexres_resolution8);
226  validate_flexres_start_angle(conf.flexres_start_angle1, conf.flexres_start_angle2);
227  validate_flexres_start_angle(conf.flexres_start_angle2, conf.flexres_start_angle3);
228  validate_flexres_start_angle(conf.flexres_start_angle3, conf.flexres_start_angle4);
229  validate_flexres_start_angle(conf.flexres_start_angle4, conf.flexres_start_angle5);
230  validate_flexres_start_angle(conf.flexres_start_angle5, conf.flexres_start_angle6);
231  validate_flexres_start_angle(conf.flexres_start_angle6, conf.flexres_start_angle7);
232  validate_flexres_start_angle(conf.flexres_start_angle7, conf.flexres_start_angle8);
233 }
234 
236 {
237  // Check that res is one of 4/8/16/32. This has to be checked manually here, since
238  // the dynamic parameter is an int with min 4 and max 32, so dynamic reconfigure
239  // doesn't prevent the user from setting an invalid value inside that range.
240  // (Values outside that range will still be clamped automatically.)
241 
242  switch (res)
243  {
244  case SickLDMRSDriver_Res0125:
245  case SickLDMRSDriver_Res0250:
246  case SickLDMRSDriver_Res0500:
247  case SickLDMRSDriver_Res1000:
248  break;
249  default:
250  ROS_WARN("Invalid flexres resolution %d! Setting to 32 (= 1 degree).", res);
251  res = SickLDMRSDriver_Res1000;
252  break;
253  }
254 }
255 
256 void SickLDMRS::validate_flexres_start_angle(double &angle1, double &angle2)
257 {
258  // make sure the angles are monotonically decreasing
259  if (angle2 > angle1)
260  {
261  angle2 = angle1;
262  }
263 }
264 
265 void SickLDMRS::pubObjects(datatypes::ObjectList &objects)
266 {
267  sick_ldmrs_msgs::ObjectArray oa;
268  oa.header.frame_id = config_.frame_id;
269  // not using time stamp from scanner here, because it is delayed by up to 1.5 seconds
270  oa.header.stamp = ros::Time::now();
271  oa.objects.resize(objects.size());
272 
273  for (int i = 0; i < objects.size(); i++)
274  {
275  oa.objects[i].id = objects[i].getObjectId();
276  oa.objects[i].tracking_time = ros::Time::now() - ros::Duration(objects[i].getObjectAge() / expected_frequency_);
277  oa.objects[i].last_seen = ros::Time::now() - ros::Duration(objects[i].getHiddenStatusAge() / expected_frequency_);
278  oa.objects[i].velocity.twist.linear.x = objects[i].getAbsoluteVelocity().getX();
279  oa.objects[i].velocity.twist.linear.y = objects[i].getAbsoluteVelocity().getY();
280  oa.objects[i].velocity.twist.linear.x = objects[i].getAbsoluteVelocity().getX();
281  oa.objects[i].velocity.twist.linear.y = objects[i].getAbsoluteVelocity().getY();
282  oa.objects[i].velocity.covariance[0] = objects[i].getAbsoluteVelocitySigma().getX();
283  oa.objects[i].velocity.covariance[7] = objects[i].getAbsoluteVelocitySigma().getX();
284 
285  oa.objects[i].bounding_box_center.position.x = objects[i].getBoundingBoxCenter().getX();
286  oa.objects[i].bounding_box_center.position.y = objects[i].getBoundingBoxCenter().getY();
287  oa.objects[i].bounding_box_size.x = objects[i].getBoundingBox().getX();
288  oa.objects[i].bounding_box_size.y = objects[i].getBoundingBox().getY();
289 
290  oa.objects[i].object_box_center.pose.position.x = objects[i].getCenterPoint().getX();
291  oa.objects[i].object_box_center.pose.position.y = objects[i].getCenterPoint().getY();
292  oa.objects[i].object_box_center.pose.orientation = tf::createQuaternionMsgFromYaw(objects[i].getCourseAngle());
293  oa.objects[i].object_box_center.covariance[0] = objects[i].getCenterPointSigma().getX();
294  oa.objects[i].object_box_center.covariance[7] = objects[i].getCenterPointSigma().getY();
295  oa.objects[i].object_box_center.covariance[35] = objects[i].getCourseAngleSigma();
296  oa.objects[i].object_box_size.x = objects[i].getObjectBox().getX();
297  oa.objects[i].object_box_size.y = objects[i].getObjectBox().getY();
298 
299  datatypes::Polygon2D contour = objects[i].getContourPoints();
300  oa.objects[i].contour_points.resize(contour.size());
301  for (int j = 0; j < contour.size(); j++)
302  {
303  oa.objects[i].contour_points[j].x = contour[j].getX();
304  oa.objects[i].contour_points[j].y = contour[j].getY();
305  }
306 
307  //std::cout << objects[i].toString() << std::endl;
308  }
309 
310  object_pub_.publish(oa);
311 }
312 
314 {
315  devices::LDMRS* ldmrs;
316  ldmrs = dynamic_cast<devices::LDMRS*>(manager_->getFirstDeviceByType(Sourcetype_LDMRS));
317  if (ldmrs == NULL)
318  {
319  ROS_WARN("isUpsideDown: no connection to LDMRS!");
320  return true;
321  }
322 
323  UINT32 code;
324  ldmrs->getParameter(devices::ParaUpsideDownMode, &code);
325  return (code != 0);
326 }
327 
329 {
330  devices::LDMRS* ldmrs;
331  ldmrs = dynamic_cast<devices::LDMRS*>(manager_->getFirstDeviceByType(Sourcetype_LDMRS));
332  if (ldmrs == NULL)
333  {
334  ROS_WARN("printFlexResError: no connection to LDMRS!");
335  return;
336  }
337 
338  UINT32 code;
339  ldmrs->getParameter(devices::ParaDetailedError, &code);
340  std::string msg = flexres_err_to_string(code);
341  ROS_ERROR("FlexRes detailed error: %s", msg.c_str());
342 }
343 
344 void SickLDMRS::update_config(SickLDMRSDriverConfig &new_config, uint32_t level)
345 {
346  validate_config(new_config);
347  config_ = new_config;
348 
349  if (!initialized_)
350  return;
351 
352  ROS_INFO("Updating config...");
353 
354  devices::LDMRS* ldmrs;
355  ldmrs = dynamic_cast<devices::LDMRS*>(manager_->getFirstDeviceByType(Sourcetype_LDMRS));
356  if (ldmrs == NULL)
357  {
358  ROS_WARN("update_config: no connection to LDMRS!");
359  return;
360  }
361 
362  // TODO: if (new_config.start_angle < config_.end_angle): first update end angle,
363  // then start angle to ensure that always start_angle > end_angle; see comments
364  // in LuxBase::cmd_setScanAngles().
365  if (!ldmrs->setScanAngles(new_config.start_angle, new_config.end_angle))
366  ROS_WARN("Sending param not successful: ");
367 
368  switch (config_.scan_frequency)
369  {
370  case SickLDMRSDriver_ScanFreq1250:
371  expected_frequency_ = 12.5d;
372  break;
373  case SickLDMRSDriver_ScanFreq2500:
374  expected_frequency_ = 25.0d;
375  break;
376  case SickLDMRSDriver_ScanFreq5000:
377  expected_frequency_ = 50.0d;
378  break;
379  default:
380  ROS_ERROR("Unknown scan frequency: %i", config_.scan_frequency);
381  break;
382  }
383 
384  if (!ldmrs->setScanFrequency(expected_frequency_))
385  ROS_WARN("Sending param not successful: ScanFrequency");
386  if (!ldmrs->setSyncAngleOffset(config_.sync_angle_offset))
387  ROS_WARN("Sending param not successful: SyncAngleOffset");
388  if (!ldmrs->setParameter(devices::ParaContourPointDensity, config_.contour_point_density))
389  ROS_WARN("Sending param not successful: ContourPointDensity");
390  if (!ldmrs->setParameter(devices::ParaMinimumObjectAge, config_.min_object_age))
391  ROS_WARN("Sending param not successful: MinimumObjectAge");
392  if (!ldmrs->setParameter(devices::ParaMaximumPredictionAge, config_.max_prediction_age))
393  ROS_WARN("Sending param not successful: MaximumPredictionAge");
394  if (!ldmrs->setParameter(devices::ParaRangeReduction, config_.layer_range_reduction))
395  ROS_WARN("Sending param not successful: RangeReduction");
396  if (!ldmrs->setParameter(devices::ParaIgnoreNearRange, config_.ignore_near_range ? 1 : 0))
397  ROS_WARN("Sending param not successful: IgnoreNearRange");
398  if (!ldmrs->setParameter(devices::ParaSensitivityControl, config_.sensitivity_control ? 1 : 0))
399  ROS_WARN("Sending param not successful: SensitivityControl");
400 
401  if (config_.angular_resolution_type == SickLDMRSDriver_FlexRes)
402  {
403  std::vector<std::pair<int, int> > res_map, res_map_filtered;
404  res_map.push_back(std::pair<int, int>(round(config_.flexres_start_angle1 * rad2deg * 32.0), config_.flexres_resolution1));
405  res_map.push_back(std::pair<int, int>(round(config_.flexres_start_angle2 * rad2deg * 32.0), config_.flexres_resolution2));
406  res_map.push_back(std::pair<int, int>(round(config_.flexres_start_angle3 * rad2deg * 32.0), config_.flexres_resolution3));
407  res_map.push_back(std::pair<int, int>(round(config_.flexres_start_angle4 * rad2deg * 32.0), config_.flexres_resolution4));
408  res_map.push_back(std::pair<int, int>(round(config_.flexres_start_angle5 * rad2deg * 32.0), config_.flexres_resolution5));
409  res_map.push_back(std::pair<int, int>(round(config_.flexres_start_angle6 * rad2deg * 32.0), config_.flexres_resolution6));
410  res_map.push_back(std::pair<int, int>(round(config_.flexres_start_angle7 * rad2deg * 32.0), config_.flexres_resolution7));
411  res_map.push_back(std::pair<int, int>(round(config_.flexres_start_angle8 * rad2deg * 32.0), config_.flexres_resolution8));
412 
413  // --- skip zero-length sectors
414  for (int i = 0; i < res_map.size() - 1; ++i)
415  {
416  if (res_map[i].first > res_map[i + 1].first)
417  {
418  res_map_filtered.push_back(res_map[i]);
419  }
420  }
421  if (res_map[7].first > (-1918)) // -1918 = minimum start angle
422  {
423  res_map_filtered.push_back(res_map[7]);
424  }
425 
426  // --- ensure constraints are met
427  int shots_per_scan = 0;
428  double sum_res0125 = 0;
429  for (int i = 0; i < res_map_filtered.size() - 1; ++i)
430  {
431  // sector angle in degrees
432  double sector_angle = (res_map_filtered[i].first - res_map_filtered[i + 1].first) / 32.0;
433 
434  shots_per_scan += sector_angle * 32.0 / res_map_filtered[i].second;
435  if (res_map_filtered[i].second == SickLDMRSDriver_Res0125)
436  {
437  sum_res0125 += sector_angle;
438  }
439  }
440 
441  if (shots_per_scan > 440)
442  {
443  ROS_WARN("FlexRes: The number of shots per scan must be at most 440. Not updating FlexRes config!");
444  return;
445  }
446  if (sum_res0125 > 20.0)
447  {
448  ROS_WARN("FlexRes: The sectors with a resolution of 0.125 deg must not sum up to more than 20 deg. Not updating FlexRes config!");
449  return;
450  }
451 
452  // --- switch to constant resolution
453  // when applying FlexRes params, the angular resolution type has to be set
454  // to something other than FlexRes
455  if (!ldmrs->setParameter(devices::ParaAngularResolutionType, SickLDMRSDriver_ConstantRes))
456  ROS_WARN("Sending param not successful: AngularResolutionType");
457 
458  // sleep 10 seconds so that new config is applied by the scanner
459  usleep(10e6);
460 
461  // --- send FlexRes params to scanner
462  if (!ldmrs->setParameter(devices::ParaNumSectors, res_map_filtered.size()))
464 
465  for (int i = 0; i < res_map_filtered.size(); ++i)
466  {
467  // set sector start angle
468  if (!ldmrs->setParameter((devices::MrsParameterId)(0x4001 + i), res_map_filtered[i].first))
470 
471  // set sector resolution
472  if (!ldmrs->setParameter((devices::MrsParameterId)(0x4009 + i), res_map_filtered[i].second))
474  }
475  }
476 
477  // set angular resolution type *after* FlexRes config!
478  if (!ldmrs->setParameter(devices::ParaAngularResolutionType, config_.angular_resolution_type))
479  ROS_WARN("Sending param not successful: AngularResolutionType");
480 
481  ROS_INFO("... done updating config.");
482 }
483 
484 std::string SickLDMRS::flexres_err_to_string(const UINT32 code) const
485 {
486  switch (code)
487  {
488  case devices::ErrFlexResNumShotsInvalid:
489  return "The number of shots per scan is higher than 440.";
490  case devices::ErrFlexResSizeOneEighthSectorInvalid:
491  return "The sectors with a resolution of 0.125 deg sum up to more than 20 deg.";
492  case devices::ErrFlexResFreqInvalid:
493  return "The scan frequency is not 12.5Hz.";
494  case devices::ErrFlexResSectorsOverlapping:
495  return "The start angles of the sectors decrease not strictly monotone.";
496  case devices::ErrFlexResScannerNotIdle:
497  return "Could not set FlexRes parameter because the sensor is in flex res mode and not idle.";
498  case devices::ErrFlexResResolutionInvalid:
499  return "The resolution of one sector is not 4, 8, 16 or 32 (0.125 deg, 0.25 deg, 0.5 deg, 1 deg).";
500  case devices::ErrFlexResNumSectorsInvalid:
501  return "The number of sectors is larger than 8.";
502  default:
503  std::ostringstream ss;
504  ss << "UNKNOWN ERROR CODE (" << code << ")";
505  return ss.str();
506  }
507 }
508 
509 } /* namespace sick_ldmrs_driver */
510 
511 
512 int main(int argc, char **argv)
513 {
514  ros::init(argc, argv, "sick_ldmrs_node");
516  = boost::make_shared<diagnostic_updater::Updater>();
517 
518  // The MRS-App connects to an MRS, reads its configuration and receives all incoming data.
519  // First, create the manager object. The manager handles devices, collects
520  // device data and forwards it to the application(s).
521  ROS_INFO("Creating the manager.");
522  Manager manager;
523 
524  // Add the application. As the devices may send configuration data only once
525  // at startup, the applications must be present before the devices are
526  // started.
527  Sourcetype type;
528  std::string name;
529  UINT16 id;
530  bool result = false;
531 
532  ROS_INFO("Adding the application SickLDMRS.");
533  name = "Sick LDMRS ROS Driver App";
534  id = 1356;
535 
536  sick_ldmrs_driver::SickLDMRS app(&manager, diagnostics);
537  app.setApplicationName(name);
538 
539  result = manager.addApplication(&app, id);
540  if (result == false)
541  {
542  ROS_ERROR("Failed to add application %s, aborting!", name.c_str());
543  return EXIT_FAILURE;
544  }
545  ROS_INFO("Application is running.");
546 
547  //
548  // Add and run the sensor
549  //
550  // The MRS device could be configured like this:
551  // m_weWantScanData: true
552  // m_weWantObjectData: true
553  // m_weWantFieldData: false
554  // m_weWantScanDataFromSopas: false
555  ROS_INFO("Adding the LDMRS device.");
556  devices::LDMRS* ldmrs = new devices::LDMRS(&manager);
557  ldmrs->setWeWantObjectData(true);
558  std::string hostname;
559  ros::NodeHandle nh("~");
560  nh.param<std::string>("hostname", hostname, "192.168.0.1");
561  ROS_INFO("Set IP address to %s", hostname.c_str());
562  ldmrs->setIpAddress(hostname);
563  name = "LDMRS-1";
564  id = 1;
565  result = manager.addAndRunDevice(ldmrs, name, id);
566  if (result == false)
567  {
568  ROS_ERROR("Failed to add device %s, aborting!", name.c_str());
569  return EXIT_FAILURE;
570  }
571 
572  std::string serial_number = ldmrs->getSerialNumber();
573  diagnostics->setHardwareID(serial_number);
574 
575  ROS_INFO("LD-MRS Firmware version is %s", ldmrs->getFirmwareVersion().c_str());
576 
577  // we need to initialize the app after setting up the ldmrs device
578  app.init();
579 
580  ROS_INFO("%s is initialized.", ros::this_node::getName().c_str());
581 
582  ros::Rate r(10.0);
583  while (ros::ok())
584  {
585  ros::spinOnce();
586  diagnostics->update();
587  r.sleep();
588  }
589 
590  return EXIT_SUCCESS;
591 }
app
msg
diagnostic_updater::DiagnosedPublisher< sensor_msgs::PointCloud2 > * diagnosticPub_
int main(int argc, char **argv)
SickLDMRS(Manager *manager, boost::shared_ptr< diagnostic_updater::Updater > diagnostics)
void publish(const boost::shared_ptr< M > &message) const
dynamic_reconfigure::Server< SickLDMRSDriverConfig > dynamic_reconfigure_server_
f
void summary(unsigned char lvl, const std::string s)
void setData(BasicData &data)
void produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
void validate_flexres_resolution(int &res)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
void validate_config(SickLDMRSDriverConfig &conf)
boost::shared_ptr< diagnostic_updater::Updater > diagnostics_
std::string flexres_err_to_string(const UINT32 code) const
virtual void publish(const boost::shared_ptr< T > &message)
#define ROS_WARN(...)
void update_config(SickLDMRSDriverConfig &new_config, uint32_t level=0)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
void validate_flexres_start_angle(double &angle1, double &angle2)
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
static Time now()
void add(const std::string &key, const T &val)
ROSCPP_DECL void spinOnce()
void pubObjects(datatypes::ObjectList &objects)
#define ROS_ERROR(...)
SickLDMRSDriverConfig config_
#define ROS_DEBUG(...)


sick_ldmrs_driver
Author(s): Martin Günther , Jochen Sprickerhof
autogenerated on Sun Oct 25 2020 03:33:55