sick_tim_common.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2013, Osnabrück University
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Osnabrück University nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  *  Created on: 24.05.2012
00030  *
00031  *      Authors:
00032  *         Jochen Sprickerhof <jochen@sprickerhof.de>
00033  *         Martin Günther <mguenthe@uos.de>
00034  *
00035  * Based on the TiM communication example by SICK AG.
00036  *
00037  */
00038 
00039 #include <sick_tim/sick_tim_common.h>
00040 
00041 #include <cstdio>
00042 #include <cstring>
00043 
00044 namespace sick_tim
00045 {
00046 
00047 SickTimCommon::SickTimCommon(AbstractParser* parser) :
00048     diagnosticPub_(NULL), expectedFrequency_(15.0), parser_(parser)
00049     // FIXME All Tims have 15Hz?
00050 {
00051   dynamic_reconfigure::Server<sick_tim::SickTimConfig>::CallbackType f;
00052   f = boost::bind(&sick_tim::SickTimCommon::update_config, this, _1, _2);
00053   dynamic_reconfigure_server_.setCallback(f);
00054 
00055   // datagram publisher (only for debug)
00056   ros::NodeHandle pn("~");
00057   pn.param<bool>("publish_datagram", publish_datagram_, false);
00058   if (publish_datagram_)
00059     datagram_pub_ = nh_.advertise<std_msgs::String>("datagram", 1000);
00060 
00061   // scan publisher
00062   pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 1000);
00063 
00064   diagnostics_.setHardwareID("none");   // set from device after connection
00065   diagnosticPub_ = new diagnostic_updater::DiagnosedPublisher<sensor_msgs::LaserScan>(pub_, diagnostics_,
00066           // frequency should be target +- 10%.
00067           diagnostic_updater::FrequencyStatusParam(&expectedFrequency_, &expectedFrequency_, 0.1, 10),
00068           // timestamp delta can be from 0.0 to 1.3x what it ideally is.
00069           diagnostic_updater::TimeStampStatusParam(-1, 1.3 * 1.0/expectedFrequency_ - config_.time_offset));
00070   ROS_ASSERT(diagnosticPub_ != NULL);
00071 }
00072 
00073 int SickTimCommon::stop_scanner()
00074 {
00075   /*
00076    * Stop streaming measurements
00077    */
00078   const char requestScanData0[] = {"\x02sEN LMDscandata 0\x03\0"};
00079   int result = sendSOPASCommand(requestScanData0, NULL);
00080   if (result != 0)
00081     // use printf because we cannot use ROS_ERROR from the destructor
00082     printf("\nSOPAS - Error stopping streaming scan data!\n");
00083   else
00084     printf("\nSOPAS - Stopped streaming scan data.\n");
00085 
00086   return result;
00087 }
00088 
00089 bool SickTimCommon::rebootScanner()
00090 {
00091   /*
00092    * Set Maintenance access mode to allow reboot to be sent
00093    */
00094   std::vector<unsigned char> access_reply;
00095   int result = sendSOPASCommand("\x02sMN SetAccessMode 03 F4724744\x03\0", &access_reply);
00096   if (result != 0)
00097   {
00098     ROS_ERROR("SOPAS - Error setting access mode");
00099     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error setting access mode.");
00100     return false;
00101   }
00102   std::string access_reply_str = replyToString(access_reply);
00103   if (access_reply_str != "sAN SetAccessMode 1")
00104   {
00105     ROS_ERROR_STREAM("SOPAS - Error setting access mode, unexpected response : " << access_reply_str);
00106     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error setting access mode.");
00107     return false;
00108   }
00109 
00110   /*
00111    * Send reboot command
00112    */
00113   std::vector<unsigned char> reboot_reply;
00114   result = sendSOPASCommand("\x02sMN mSCreboot\x03\0", &reboot_reply);
00115   if (result != 0)
00116   {
00117     ROS_ERROR("SOPAS - Error rebooting scanner");
00118     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error rebooting device.");
00119     return false;
00120   }
00121   std::string reboot_reply_str = replyToString(reboot_reply);
00122   if (reboot_reply_str != "sAN mSCreboot")
00123   {
00124     ROS_ERROR_STREAM("SOPAS - Error rebooting scanner, unexpected response : " << reboot_reply_str);
00125     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error setting access mode.");
00126     return false;
00127   }
00128 
00129   ROS_INFO("SOPAS - Rebooted scanner");
00130 
00131   // Wait a few seconds after rebooting
00132   ros::Duration(15.0).sleep();
00133 
00134   return true;
00135 }
00136 
00137 SickTimCommon::~SickTimCommon()
00138 {
00139   delete diagnosticPub_;
00140 
00141   printf("sick_tim driver exiting.\n");
00142 }
00143 
00144 
00145 int SickTimCommon::init()
00146 {
00147   int result = init_device();
00148   if(result != 0) {
00149       ROS_FATAL("Failed to init device: %d", result);
00150       return result;
00151   }
00152   result = init_scanner();
00153   if(result != 0) {
00154       ROS_FATAL("Failed to init scanner: %d", result);
00155   }
00156   return result;
00157 }
00158 
00159 int SickTimCommon::init_scanner()
00160 {
00161   /*
00162    * Read the SOPAS variable 'DeviceIdent' by index.
00163    */
00164   const char requestDeviceIdent[] = "\x02sRI0\x03\0";
00165   std::vector<unsigned char> identReply;
00166   int result = sendSOPASCommand(requestDeviceIdent, &identReply);
00167   if (result != 0)
00168   {
00169     ROS_ERROR("SOPAS - Error reading variable 'DeviceIdent'.");
00170     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'DeviceIdent'.");
00171   }
00172 
00173   /*
00174    * Read the SOPAS variable 'SerialNumber' by name.
00175    */
00176   const char requestSerialNumber[] = "\x02sRN SerialNumber\x03\0";
00177   std::vector<unsigned char> serialReply;
00178   result = sendSOPASCommand(requestSerialNumber, &serialReply);
00179   if (result != 0)
00180   {
00181     ROS_ERROR("SOPAS - Error reading variable 'SerialNumber'.");
00182     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'SerialNumber'.");
00183   }
00184 
00185   // set hardware ID based on DeviceIdent and SerialNumber
00186   std::string identStr = replyToString(identReply);
00187   std::string serialStr = replyToString(serialReply);
00188   diagnostics_.setHardwareID(identStr + " " + serialStr);
00189 
00190   if (!isCompatibleDevice(identStr))
00191     return ExitFatal;
00192 
00193   /*
00194    * Read the SOPAS variable 'FirmwareVersion' by name.
00195    */
00196   const char requestFirmwareVersion[] = {"\x02sRN FirmwareVersion\x03\0"};
00197   result = sendSOPASCommand(requestFirmwareVersion, NULL);
00198   if (result != 0)
00199   {
00200     ROS_ERROR("SOPAS - Error reading variable 'FirmwareVersion'.");
00201     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'FirmwareVersion'.");
00202   }
00203 
00204   /*
00205    * Read Device State
00206    */
00207   const char requestDeviceState[] = {"\x02sRN SCdevicestate\x03\0"};
00208   std::vector<unsigned char> deviceStateReply;
00209   result = sendSOPASCommand(requestDeviceState, &deviceStateReply);
00210   if (result != 0)
00211   {
00212     ROS_ERROR("SOPAS - Error reading variable 'devicestate'.");
00213     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'devicestate'.");
00214   }
00215   std::string deviceStateReplyStr = replyToString(deviceStateReply);
00216 
00217   /*
00218    * Process device state, 0=Busy, 1=Ready, 2=Error
00219    * If configuration parameter is set, try resetting device in error state
00220    */
00221   if (deviceStateReplyStr == "sRA SCdevicestate 0")
00222   {
00223     ROS_WARN("Laser is busy");
00224   }
00225   else if (deviceStateReplyStr == "sRA SCdevicestate 1")
00226   {
00227     ROS_DEBUG("Laser is ready");
00228   }
00229   else if (deviceStateReplyStr == "sRA SCdevicestate 2")
00230   {
00231     ROS_ERROR_STREAM("Laser reports error state : " << deviceStateReplyStr);
00232     if (config_.auto_reboot)
00233     {
00234       rebootScanner();
00235     }
00236   }
00237   else
00238   {
00239     ROS_WARN_STREAM("Laser reports unknown devicestate : " << deviceStateReplyStr);
00240   }
00241 
00242   /*
00243    * Start streaming 'LMDscandata'.
00244    */
00245   const char requestScanData[] = {"\x02sEN LMDscandata 1\x03\0"};
00246   result = sendSOPASCommand(requestScanData, NULL);
00247   if (result != 0)
00248   {
00249     ROS_ERROR("SOPAS - Error starting to stream 'LMDscandata'.");
00250     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error starting to stream 'LMDscandata'.");
00251     return ExitError;
00252   }
00253 
00254   return ExitSuccess;
00255 }
00256 
00257 std::string sick_tim::SickTimCommon::replyToString(const std::vector<unsigned char> &reply)
00258 {
00259   std::string reply_str;
00260   for (std::vector<unsigned char>::const_iterator it = reply.begin(); it != reply.end(); it++)
00261   {
00262     if (*it > 13) // filter control characters for display
00263     {
00264       reply_str.push_back(*it);
00265     }
00266   }
00267   return reply_str;
00268 }
00269 
00270 bool sick_tim::SickTimCommon::isCompatibleDevice(const std::string identStr) const
00271 {
00272   char device_string[7];
00273   int version_major = -1;
00274   int version_minor = -1;
00275 
00276   if (sscanf(identStr.c_str(), "sRA 0 6 %6s E V%d.%d", device_string,
00277              &version_major, &version_minor) == 3
00278       && strncmp("TiM3", device_string, 4) == 0
00279       && version_major >= 2 && version_minor >= 50)
00280   {
00281     ROS_ERROR("This scanner model/firmware combination does not support ranging output!");
00282     ROS_ERROR("Supported scanners: TiM5xx: all firmware versions; TiM3xx: firmware versions < V2.50.");
00283     ROS_ERROR("This is a %s, firmware version %d.%d", device_string, version_major, version_minor);
00284 
00285     return false;
00286   }
00287   return true;
00288 }
00289 
00290 int SickTimCommon::loopOnce()
00291 {
00292   diagnostics_.update();
00293 
00294   unsigned char receiveBuffer[65536];
00295   int actual_length = 0;
00296   static unsigned int iteration_count = 0;
00297 
00298   int result = get_datagram(receiveBuffer, 65536, &actual_length);
00299   if (result != 0)
00300   {
00301       ROS_ERROR("Read Error when getting datagram: %i.", result);
00302       diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Read Error when getting datagram.");
00303       return ExitError; // return failure to exit node
00304   }
00305   if(actual_length <= 0)
00306       return ExitSuccess; // return success to continue looping
00307 
00308   // ----- if requested, skip frames
00309   if (iteration_count++ % (config_.skip + 1) != 0)
00310     return ExitSuccess;
00311 
00312   if (publish_datagram_)
00313   {
00314     std_msgs::String datagram_msg;
00315     datagram_msg.data = std::string(reinterpret_cast<char*>(receiveBuffer));
00316     datagram_pub_.publish(datagram_msg);
00317   }
00318 
00319   sensor_msgs::LaserScan msg;
00320 
00321   /*
00322    * datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
00323    */
00324   char* buffer_pos = (char*)receiveBuffer;
00325   char *dstart, *dend;
00326   while( (dstart = strchr(buffer_pos, 0x02)) && (dend = strchr(dstart + 1, 0x03)) )
00327   {
00328     size_t dlength = dend - dstart;
00329     *dend = '\0';
00330     dstart++;
00331     int success = parser_->parse_datagram(dstart, dlength, config_, msg);
00332     if (success == ExitSuccess)
00333       diagnosticPub_->publish(msg);
00334     buffer_pos = dend + 1;
00335   }
00336 
00337   return ExitSuccess; // return success to continue looping
00338 }
00339 
00340 void SickTimCommon::check_angle_range(SickTimConfig &conf)
00341 {
00342   if (conf.min_ang > conf.max_ang)
00343   {
00344     ROS_WARN("Maximum angle must be greater than minimum angle. Adjusting min_ang.");
00345     conf.min_ang = conf.max_ang;
00346   }
00347 }
00348 
00349 void SickTimCommon::update_config(sick_tim::SickTimConfig &new_config, uint32_t level)
00350 {
00351   check_angle_range(new_config);
00352   config_ = new_config;
00353 }
00354 
00355 } /* namespace sick_tim */


sick_tim
Author(s): Jochen Sprickerhof , Martin Günther , Sebastian Pütz
autogenerated on Thu May 9 2019 02:32:02