sick_tim_common.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013, Osnabrück University
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 Osnabrück University 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: 24.05.2012
30  *
31  * Authors:
32  * Jochen Sprickerhof <jochen@sprickerhof.de>
33  * Martin Günther <mguenthe@uos.de>
34  *
35  * Based on the TiM communication example by SICK AG.
36  *
37  */
38 
40 
41 #include <cstdio>
42 #include <cstring>
43 
44 namespace sick_tim
45 {
46 
48  diagnosticPub_(NULL), expectedFrequency_(15.0), parser_(parser)
49  // FIXME All Tims have 15Hz?
50 {
51  dynamic_reconfigure::Server<sick_tim::SickTimConfig>::CallbackType f;
52  f = boost::bind(&sick_tim::SickTimCommon::update_config, this, _1, _2);
53  dynamic_reconfigure_server_.setCallback(f);
54 
55  // datagram publisher (only for debug)
56  ros::NodeHandle pn("~");
57  pn.param<bool>("publish_datagram", publish_datagram_, false);
58  if (publish_datagram_)
59  datagram_pub_ = nh_.advertise<std_msgs::String>("datagram", 1000);
60 
61  // scan publisher
62  pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 1000);
63 
64  diagnostics_.setHardwareID("none"); // set from device after connection
66  // frequency should be target +- 10%.
68  // timestamp delta can be from 0.0 to 1.3x what it ideally is.
70  ROS_ASSERT(diagnosticPub_ != NULL);
71 }
72 
74 {
75  /*
76  * Stop streaming measurements
77  */
78  const char requestScanData0[] = {"\x02sEN LMDscandata 0\x03\0"};
79  int result = sendSOPASCommand(requestScanData0, NULL);
80  if (result != 0)
81  // use printf because we cannot use ROS_ERROR from the destructor
82  printf("\nSOPAS - Error stopping streaming scan data!\n");
83  else
84  printf("\nSOPAS - Stopped streaming scan data.\n");
85 
86  return result;
87 }
88 
90 {
91  /*
92  * Set Maintenance access mode to allow reboot to be sent
93  */
94  std::vector<unsigned char> access_reply;
95  int result = sendSOPASCommand("\x02sMN SetAccessMode 03 F4724744\x03\0", &access_reply);
96  if (result != 0)
97  {
98  ROS_ERROR("SOPAS - Error setting access mode");
99  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error setting access mode.");
100  return false;
101  }
102  std::string access_reply_str = replyToString(access_reply);
103  if (access_reply_str != "sAN SetAccessMode 1")
104  {
105  ROS_ERROR_STREAM("SOPAS - Error setting access mode, unexpected response : " << access_reply_str);
106  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error setting access mode.");
107  return false;
108  }
109 
110  /*
111  * Send reboot command
112  */
113  std::vector<unsigned char> reboot_reply;
114  result = sendSOPASCommand("\x02sMN mSCreboot\x03\0", &reboot_reply);
115  if (result != 0)
116  {
117  ROS_ERROR("SOPAS - Error rebooting scanner");
118  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error rebooting device.");
119  return false;
120  }
121  std::string reboot_reply_str = replyToString(reboot_reply);
122  if (reboot_reply_str != "sAN mSCreboot")
123  {
124  ROS_ERROR_STREAM("SOPAS - Error rebooting scanner, unexpected response : " << reboot_reply_str);
125  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error setting access mode.");
126  return false;
127  }
128 
129  ROS_INFO("SOPAS - Rebooted scanner");
130 
131  // Wait a few seconds after rebooting
132  ros::Duration(15.0).sleep();
133 
134  return true;
135 }
136 
138 {
139  delete diagnosticPub_;
140 
141  printf("sick_tim driver exiting.\n");
142 }
143 
144 
146 {
147  int result = init_device();
148  if(result != 0) {
149  ROS_FATAL("Failed to init device: %d", result);
150  return result;
151  }
152  result = init_scanner();
153  if(result != 0) {
154  ROS_FATAL("Failed to init scanner: %d", result);
155  }
156  return result;
157 }
158 
160 {
161  /*
162  * Read the SOPAS variable 'DeviceIdent' by index.
163  */
164  const char requestDeviceIdent[] = "\x02sRI0\x03\0";
165  std::vector<unsigned char> identReply;
166  int result = sendSOPASCommand(requestDeviceIdent, &identReply);
167  if (result != 0)
168  {
169  ROS_ERROR("SOPAS - Error reading variable 'DeviceIdent'.");
170  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'DeviceIdent'.");
171  }
172 
173  /*
174  * Read the SOPAS variable 'SerialNumber' by name.
175  */
176  const char requestSerialNumber[] = "\x02sRN SerialNumber\x03\0";
177  std::vector<unsigned char> serialReply;
178  result = sendSOPASCommand(requestSerialNumber, &serialReply);
179  if (result != 0)
180  {
181  ROS_ERROR("SOPAS - Error reading variable 'SerialNumber'.");
182  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'SerialNumber'.");
183  }
184 
185  // set hardware ID based on DeviceIdent and SerialNumber
186  std::string identStr = replyToString(identReply);
187  std::string serialStr = replyToString(serialReply);
188  diagnostics_.setHardwareID(identStr + " " + serialStr);
189 
190  if (!isCompatibleDevice(identStr))
191  return ExitFatal;
192 
193  /*
194  * Read the SOPAS variable 'FirmwareVersion' by name.
195  */
196  const char requestFirmwareVersion[] = {"\x02sRN FirmwareVersion\x03\0"};
197  result = sendSOPASCommand(requestFirmwareVersion, NULL);
198  if (result != 0)
199  {
200  ROS_ERROR("SOPAS - Error reading variable 'FirmwareVersion'.");
201  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'FirmwareVersion'.");
202  }
203 
204  /*
205  * Read Device State
206  */
207  const char requestDeviceState[] = {"\x02sRN SCdevicestate\x03\0"};
208  std::vector<unsigned char> deviceStateReply;
209  result = sendSOPASCommand(requestDeviceState, &deviceStateReply);
210  if (result != 0)
211  {
212  ROS_ERROR("SOPAS - Error reading variable 'devicestate'.");
213  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'devicestate'.");
214  }
215  std::string deviceStateReplyStr = replyToString(deviceStateReply);
216 
217  /*
218  * Process device state, 0=Busy, 1=Ready, 2=Error
219  * If configuration parameter is set, try resetting device in error state
220  */
221  if (deviceStateReplyStr == "sRA SCdevicestate 0")
222  {
223  ROS_WARN("Laser is busy");
224  }
225  else if (deviceStateReplyStr == "sRA SCdevicestate 1")
226  {
227  ROS_DEBUG("Laser is ready");
228  }
229  else if (deviceStateReplyStr == "sRA SCdevicestate 2")
230  {
231  ROS_ERROR_STREAM("Laser reports error state : " << deviceStateReplyStr);
232  if (config_.auto_reboot)
233  {
234  rebootScanner();
235  }
236  }
237  else
238  {
239  ROS_WARN_STREAM("Laser reports unknown devicestate : " << deviceStateReplyStr);
240  }
241 
242  /*
243  * Start streaming 'LMDscandata'.
244  */
245  const char requestScanData[] = {"\x02sEN LMDscandata 1\x03\0"};
246  result = sendSOPASCommand(requestScanData, NULL);
247  if (result != 0)
248  {
249  ROS_ERROR("SOPAS - Error starting to stream 'LMDscandata'.");
250  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error starting to stream 'LMDscandata'.");
251  return ExitError;
252  }
253 
254  return ExitSuccess;
255 }
256 
257 std::string sick_tim::SickTimCommon::replyToString(const std::vector<unsigned char> &reply)
258 {
259  std::string reply_str;
260  for (std::vector<unsigned char>::const_iterator it = reply.begin(); it != reply.end(); it++)
261  {
262  if (*it > 13) // filter control characters for display
263  {
264  reply_str.push_back(*it);
265  }
266  }
267  return reply_str;
268 }
269 
270 bool sick_tim::SickTimCommon::isCompatibleDevice(const std::string identStr) const
271 {
272  char device_string[7];
273  int version_major = -1;
274  int version_minor = -1;
275 
276  if (sscanf(identStr.c_str(), "sRA 0 6 %6s E V%d.%d", device_string,
277  &version_major, &version_minor) == 3
278  && strncmp("TiM3", device_string, 4) == 0
279  && version_major >= 2 && version_minor >= 50)
280  {
281  ROS_ERROR("This scanner model/firmware combination does not support ranging output!");
282  ROS_ERROR("Supported scanners: TiM5xx: all firmware versions; TiM3xx: firmware versions < V2.50.");
283  ROS_ERROR("This is a %s, firmware version %d.%d", device_string, version_major, version_minor);
284 
285  return false;
286  }
287  return true;
288 }
289 
291 {
293 
294  unsigned char receiveBuffer[65536];
295  int actual_length = 0;
296  static unsigned int iteration_count = 0;
297 
298  int result = get_datagram(receiveBuffer, 65536, &actual_length);
299  if (result != 0)
300  {
301  ROS_ERROR("Read Error when getting datagram: %i.", result);
302  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Read Error when getting datagram.");
303  return ExitError; // return failure to exit node
304  }
305  if(actual_length <= 0)
306  return ExitSuccess; // return success to continue looping
307 
308  // ----- if requested, skip frames
309  if (iteration_count++ % (config_.skip + 1) != 0)
310  return ExitSuccess;
311 
312  if (publish_datagram_)
313  {
314  std_msgs::String datagram_msg;
315  datagram_msg.data = std::string(reinterpret_cast<char*>(receiveBuffer));
316  datagram_pub_.publish(datagram_msg);
317  }
318 
319  sensor_msgs::LaserScan msg;
320 
321  /*
322  * datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
323  */
324  char* buffer_pos = (char*)receiveBuffer;
325  char *dstart, *dend;
326  while( (dstart = strchr(buffer_pos, 0x02)) && (dend = strchr(dstart + 1, 0x03)) )
327  {
328  size_t dlength = dend - dstart;
329  *dend = '\0';
330  dstart++;
331  int success = parser_->parse_datagram(dstart, dlength, config_, msg);
332  if (success == ExitSuccess)
333  diagnosticPub_->publish(msg);
334  buffer_pos = dend + 1;
335  }
336 
337  return ExitSuccess; // return success to continue looping
338 }
339 
340 void SickTimCommon::check_angle_range(SickTimConfig &conf)
341 {
342  if (conf.min_ang > conf.max_ang)
343  {
344  ROS_WARN("Maximum angle must be greater than minimum angle. Adjusting min_ang.");
345  conf.min_ang = conf.max_ang;
346  }
347 }
348 
349 void SickTimCommon::update_config(sick_tim::SickTimConfig &new_config, uint32_t level)
350 {
351  check_angle_range(new_config);
352  config_ = new_config;
353 }
354 
355 } /* namespace sick_tim */
msg
virtual int init_device()=0
#define ROS_FATAL(...)
virtual bool rebootScanner()
Send a SOPAS command to the scanner that should cause a soft reset.
void publish(const boost::shared_ptr< M > &message) const
f
void setHardwareID(const std::string &hwid)
ros::Publisher datagram_pub_
bool sleep() const
virtual int parse_datagram(char *datagram, size_t datagram_length, SickTimConfig &config, sensor_msgs::LaserScan &msg)=0
virtual void publish(const boost::shared_ptr< T > &message)
diagnostic_updater::DiagnosedPublisher< sensor_msgs::LaserScan > * diagnosticPub_
#define ROS_WARN(...)
static std::string replyToString(const std::vector< unsigned char > &reply)
Converts reply from sendSOPASCommand to string.
dynamic_reconfigure::Server< sick_tim::SickTimConfig > dynamic_reconfigure_server_
virtual int sendSOPASCommand(const char *request, std::vector< unsigned char > *reply)=0
Send a SOPAS command to the device and print out the response to the console.
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_WARN_STREAM(args)
diagnostic_updater::Updater diagnostics_
AbstractParser * parser_
void check_angle_range(SickTimConfig &conf)
void update_config(sick_tim::SickTimConfig &new_config, uint32_t level=0)
void broadcast(int lvl, const std::string msg)
#define ROS_ERROR_STREAM(args)
#define ROS_ASSERT(cond)
SickTimCommon(AbstractParser *parser)
virtual int get_datagram(unsigned char *receiveBuffer, int bufferSize, int *actual_length)=0
Read a datagram from the device.
#define ROS_ERROR(...)
bool isCompatibleDevice(const std::string identStr) const
#define ROS_DEBUG(...)


sick_tim
Author(s): Jochen Sprickerhof , Martin Günther , Sebastian Pütz
autogenerated on Wed Jun 17 2020 04:05:36