sick_tim551_2050001.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: 14.11.2013
30  *
31  * Author:
32  * Martin Günther <mguenthe@uos.de>
33  *
34  */
35 
40 
41 int main(int argc, char **argv)
42 {
43  ros::init(argc, argv, "sick_tim551_2050001");
44  ros::NodeHandle nhPriv("~");
45 
46  // check for TCP - use if ~hostname is set.
47  bool useTCP = false;
48  std::string hostname;
49  if(nhPriv.getParam("hostname", hostname)) {
50  useTCP = true;
51  }
52  std::string port;
53  nhPriv.param<std::string>("port", port, "2112");
54 
55  int timelimit;
56  nhPriv.param("timelimit", timelimit, 5);
57 
58  bool subscribe_datagram;
59  int device_number;
60  nhPriv.param("subscribe_datagram", subscribe_datagram, false);
61  nhPriv.param("device_number", device_number, 0);
62 
64 
65  double param;
66  if (nhPriv.getParam("range_min", param))
67  {
68  parser->set_range_min(param);
69  }
70  if (nhPriv.getParam("range_max", param))
71  {
72  parser->set_range_max(param);
73  }
74  if (nhPriv.getParam("time_increment", param))
75  {
76  parser->set_time_increment(param);
77  }
78 
79  sick_tim::SickTimCommon* s = NULL;
80 
81  int result = sick_tim::ExitError;
82  while (ros::ok())
83  {
84  // Atempt to connect/reconnect
85  if (subscribe_datagram)
86  s = new sick_tim::SickTimCommonMockup(parser);
87  else if (useTCP)
88  s = new sick_tim::SickTimCommonTcp(hostname, port, timelimit, parser);
89  else
90  s = new sick_tim::SickTimCommonUsb(parser, device_number);
91  result = s->init();
92 
93  while(ros::ok() && (result == sick_tim::ExitSuccess)){
94  ros::spinOnce();
95  result = s->loopOnce();
96  }
97 
98  delete s;
99 
100  if (result == sick_tim::ExitFatal)
101  return result;
102 
103  if (ros::ok() && !subscribe_datagram && !useTCP)
104  ros::Duration(1.0).sleep(); // Only attempt USB connections once per second
105  }
106 
107  delete parser;
108  return result;
109 }
bool param(const std::string &param_name, T &param_val, const T &default_val)
XmlRpcServer s
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
int main(int argc, char **argv)
bool getParam(const std::string &key, std::string &s) const
parser
ROSCPP_DECL void spinOnce()


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