sick_tim551_2050001.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: 14.11.2013
00030  *
00031  *      Author:
00032  *         Martin Günther <mguenthe@uos.de>
00033  *
00034  */
00035 
00036 #include <sick_tim/sick_tim_common_usb.h>
00037 #include <sick_tim/sick_tim_common_tcp.h>
00038 #include <sick_tim/sick_tim_common_mockup.h>
00039 #include <sick_tim/sick_tim551_2050001_parser.h>
00040 
00041 int main(int argc, char **argv)
00042 {
00043   ros::init(argc, argv, "sick_tim551_2050001");
00044   ros::NodeHandle nhPriv("~");
00045 
00046   // check for TCP - use if ~hostname is set.
00047   bool useTCP = false;
00048   std::string hostname;
00049   if(nhPriv.getParam("hostname", hostname)) {
00050       useTCP = true;
00051   }
00052   std::string port;
00053   nhPriv.param<std::string>("port", port, "2112");
00054 
00055   int timelimit;
00056   nhPriv.param("timelimit", timelimit, 5);
00057 
00058   bool subscribe_datagram;
00059   int device_number;
00060   nhPriv.param("subscribe_datagram", subscribe_datagram, false);
00061   nhPriv.param("device_number", device_number, 0);
00062 
00063   sick_tim::SickTim5512050001Parser* parser = new sick_tim::SickTim5512050001Parser();
00064 
00065   double param;
00066   if (nhPriv.getParam("range_min", param))
00067   {
00068     parser->set_range_min(param);
00069   }
00070   if (nhPriv.getParam("range_max", param))
00071   {
00072     parser->set_range_max(param);
00073   }
00074   if (nhPriv.getParam("time_increment", param))
00075   {
00076     parser->set_time_increment(param);
00077   }
00078 
00079   sick_tim::SickTimCommon* s = NULL;
00080 
00081   int result = sick_tim::ExitError;
00082   while (ros::ok())
00083   {
00084     // Atempt to connect/reconnect
00085     if (subscribe_datagram)
00086       s = new sick_tim::SickTimCommonMockup(parser);
00087     else if (useTCP)
00088       s = new sick_tim::SickTimCommonTcp(hostname, port, timelimit, parser);
00089     else
00090       s = new sick_tim::SickTimCommonUsb(parser, device_number);
00091     result = s->init();
00092 
00093     while(ros::ok() && (result == sick_tim::ExitSuccess)){
00094       ros::spinOnce();
00095       result = s->loopOnce();
00096     }
00097 
00098     delete s;
00099 
00100     if (result == sick_tim::ExitFatal)
00101       return result;
00102 
00103     if (ros::ok() && !subscribe_datagram && !useTCP)
00104       ros::Duration(1.0).sleep(); // Only attempt USB connections once per second
00105   }
00106 
00107   delete parser;
00108   return result;
00109 }


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