sick_tim3xx_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_tim3xx/sick_tim3xx_common.h>
00040 
00041 namespace sick_tim3xx
00042 {
00043 
00044 SickTim3xxCommon::SickTim3xxCommon(AbstractParser* parser) :
00045     diagnosticPub_(NULL), expectedFrequency_(15.0), parser_(parser)
00046     // FIXME All Tims have 15Hz?
00047 {
00048   dynamic_reconfigure::Server<sick_tim3xx::SickTim3xxConfig>::CallbackType f;
00049   f = boost::bind(&sick_tim3xx::SickTim3xxCommon::update_config, this, _1, _2);
00050   dynamic_reconfigure_server_.setCallback(f);
00051 
00052   // datagram publisher (only for debug)
00053   ros::NodeHandle pn("~");
00054   pn.param<bool>("publish_datagram", publish_datagram_, false);
00055   if (publish_datagram_)
00056     datagram_pub_ = nh_.advertise<std_msgs::String>("datagram", 1000);
00057 
00058   // scan publisher
00059   pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 1000);
00060 
00061   diagnostics_.setHardwareID("none");   // set from device after connection
00062   diagnosticPub_ = new diagnostic_updater::DiagnosedPublisher<sensor_msgs::LaserScan>(pub_, diagnostics_,
00063           // frequency should be target +- 10%.
00064           diagnostic_updater::FrequencyStatusParam(&expectedFrequency_, &expectedFrequency_, 0.1, 10),
00065           // timestamp delta can be from 0.0 to 1.3x what it ideally is.
00066           diagnostic_updater::TimeStampStatusParam(-1, 1.3 * 1.0/expectedFrequency_));
00067   ROS_ASSERT(diagnosticPub_ != NULL);
00068 }
00069 
00070 int SickTim3xxCommon::stop_scanner()
00071 {
00072   /*
00073    * Stop streaming measurements
00074    */
00075   const char requestScanData0[] = {"\x02sEN LMDscandata 0\x03\0"};
00076   int result = sendSOPASCommand(requestScanData0, NULL);
00077   if (result != 0)
00078     // use printf because we cannot use ROS_ERROR from the destructor
00079     printf("\nSOPAS - Error stopping streaming scan data!\n");
00080   else
00081     printf("\nSOPAS - Stopped streaming scan data.\n");
00082 
00083   return result;
00084 }
00085 
00086 SickTim3xxCommon::~SickTim3xxCommon()
00087 {
00088   delete diagnosticPub_;
00089   delete parser_;
00090 
00091   printf("sick_tim3xx driver exiting.\n");
00092 }
00093 
00094 
00095 int SickTim3xxCommon::init()
00096 {
00097   int result = init_device();
00098   if(result != 0) {
00099       ROS_FATAL("Failed to init device: %d", result);
00100       return result;
00101   }
00102   result = init_scanner();
00103   if(result != 0) {
00104       ROS_FATAL("Failed to init scanner: %d", result);
00105   }
00106   return result;
00107 }
00108 
00109 int SickTim3xxCommon::init_scanner()
00110 {
00111   /*
00112    * Read the SOPAS variable 'DeviceIdent' by index.
00113    */
00114   const char requestDeviceIdent[] = "\x02sRI0\x03\0";
00115   std::vector<unsigned char> identReply;
00116   int result = sendSOPASCommand(requestDeviceIdent, &identReply);
00117   if (result != 0)
00118   {
00119     ROS_ERROR("SOPAS - Error reading variable 'DeviceIdent'.");
00120     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'DeviceIdent'.");
00121   }
00122 
00123   /*
00124    * Read the SOPAS variable 'SerialNumber' by name.
00125    */
00126   const char requestSerialNumber[] = "\x02sRN SerialNumber\x03\0";
00127   std::vector<unsigned char> serialReply;
00128   result = sendSOPASCommand(requestSerialNumber, &serialReply);
00129   if (result != 0)
00130   {
00131     ROS_ERROR("SOPAS - Error reading variable 'SerialNumber'.");
00132     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'SerialNumber'.");
00133   }
00134 
00135   // set hardware ID based on DeviceIdent and SerialNumber
00136   identReply.push_back(0);  // add \0 to convert to string
00137   serialReply.push_back(0);
00138   std::string identStr;
00139   for(std::vector<unsigned char>::iterator it = identReply.begin(); it != identReply.end(); it++) {
00140       if(*it > 13)  // filter control characters for display
00141         identStr.push_back(*it);
00142   }
00143   std::string serialStr;
00144   for(std::vector<unsigned char>::iterator it = serialReply.begin(); it != serialReply.end(); it++) {
00145       if(*it > 13)
00146         serialStr.push_back(*it);
00147   }
00148   diagnostics_.setHardwareID(identStr + " " + serialStr);
00149 
00150   /*
00151    * Read the SOPAS variable 'FirmwareVersion' by name.
00152    */
00153   const char requestFirmwareVersion[] = {"\x02sRN FirmwareVersion\x03\0"};
00154   result = sendSOPASCommand(requestFirmwareVersion, NULL);
00155   if (result != 0)
00156   {
00157     ROS_ERROR("SOPAS - Error reading variable 'FirmwareVersion'.");
00158     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'FirmwareVersion'.");
00159   }
00160 
00161   /*
00162    * Start streaming 'LMDscandata'.
00163    */
00164   const char requestScanData[] = {"\x02sEN LMDscandata 1\x03\0"};
00165   result = sendSOPASCommand(requestScanData, NULL);
00166   if (result != 0)
00167   {
00168     ROS_ERROR("SOPAS - Error starting to stream 'LMDscandata'.");
00169     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error starting to stream 'LMDscandata'.");
00170     return EXIT_FAILURE;
00171   }
00172 
00173   return EXIT_SUCCESS;
00174 }
00175 
00176 int SickTim3xxCommon::loopOnce()
00177 {
00178   diagnostics_.update();
00179 
00180   unsigned char receiveBuffer[65536];
00181   int actual_length = 0;
00182   static unsigned int iteration_count = 0;
00183 
00184   int result = get_datagram(receiveBuffer, 65536, &actual_length);
00185   if (result != 0)
00186   {
00187       ROS_ERROR("Read Error when getting datagram: %i.", result);
00188       diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Read Error when getting datagram.");
00189       return EXIT_FAILURE; // return failure to exit node
00190   }
00191   if(actual_length <= 0)
00192       return EXIT_SUCCESS; // return success to continue looping
00193 
00194   // ----- if requested, skip frames
00195   if (iteration_count++ % (config_.skip + 1) != 0)
00196     return EXIT_SUCCESS;
00197 
00198   if (publish_datagram_)
00199   {
00200     std_msgs::String datagram_msg;
00201     datagram_msg.data = std::string(reinterpret_cast<char*>(receiveBuffer));
00202     datagram_pub_.publish(datagram_msg);
00203   }
00204 
00205   sensor_msgs::LaserScan msg;
00206   int success = parser_->parse_datagram((char*)receiveBuffer, (size_t)actual_length, config_, msg);
00207   if (success == EXIT_SUCCESS)
00208     diagnosticPub_->publish(msg);
00209 
00210   return EXIT_SUCCESS; // return success to continue looping
00211 }
00212 
00213 void SickTim3xxCommon::check_angle_range(SickTim3xxConfig &conf)
00214 {
00215   if (conf.min_ang > conf.max_ang)
00216   {
00217     ROS_WARN("Minimum angle must be greater than maximum angle. Adjusting min_ang.");
00218     conf.min_ang = conf.max_ang;
00219   }
00220 }
00221 
00222 void SickTim3xxCommon::update_config(sick_tim3xx::SickTim3xxConfig &new_config, uint32_t level)
00223 {
00224   check_angle_range(new_config);
00225   config_ = new_config;
00226 }
00227 
00228 } /* namespace sick_tim3xx */


sick_tim3xx
Author(s): Jochen Sprickerhof, Martin Günther
autogenerated on Fri Aug 15 2014 11:54:25