getID.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 #include <urg_node/urg_c_wrapper.h>
36 #include <vector>
37 #include <string>
38 
39 #include <fcntl.h>
40 #include <stdlib.h>
41 #include <stdio.h>
42 /* gcc defined unix */
43 #ifdef unix
44 #include <unistd.h>
45 #endif
46 #ifdef WIN32
47 #include <io.h>
48 #define pipe(X) _pipe(X, 4096, O_BINARY)
49 #define fileno _fileno
50 #define dup2 _dup2
51 #define read _read
52 #endif
53 
54 std::vector<std::string> &split(const std::string &s, char delim, std::vector<std::string> &elems)
55 {
56  std::stringstream ss(s);
57  std::string item;
58  while (std::getline(ss, item, delim))
59  {
60  elems.push_back(item);
61  }
62  return elems;
63 }
64 
65 
66 std::vector<std::string> split(const std::string &s, char delim)
67 {
68  std::vector<std::string> elems;
69  split(s, delim, elems);
70  return elems;
71 }
72 
73 int
74 main(int argc, char** argv)
75 {
77 
78  if (argc < 2 || argc > 3)
79  {
80  fprintf(stderr,
81  "usage: getID /dev/ttyACM? [quiet]\nOutputs the device ID of a hokuyo at /dev/ttyACM? or IP address"
82  " (specified as 192.168.1.6:10940). Add a second argument for script friendly output.\n");
83  return 1;
84  }
85 
86  bool verbose = (argc == 2);
87 
88  int save_stdout = dup(STDOUT_FILENO);
89 
90  if (!verbose) // Block urg's prints
91  {
92  int fds[2];
93  int res;
94  char buf[256];
95  int so;
96 
97  res = pipe(fds);
98  assert(res == 0);
99 
100  so = fileno(stdout);
101  // close stdout handle and make the writable part of fds the new stdout.
102  res = dup2(fds[1], so);
103  assert(res != -1);
104  }
105 
106 
107  bool publish_intensity = false;
108  bool publish_multiecho = false;
109  bool synchronize_time = false;
110  int serial_baud = 115200;
111  int ip_port = 10940;
112  std::string ip_address = "";
113  std::string serial_port = "";
114 
115  std::vector<std::string> ip_split = split(argv[1], ':');
116  if (ip_split.size() < 2) // Not an IP address
117  {
118  serial_port = argv[1];
119  }
120  else if (ip_split.size() == 2) // IP address formatted as IP:PORT
121  {
122  ip_address = ip_split[0];
123  ip_port = atoi(ip_split[1].c_str());
124  }
125  else // Invalid
126  {
127  if (verbose)
128  {
129  printf("getID failed due to invalid specifier.\n");
130  return 1;
131  }
132  }
133 
135 
136  for (int retries = 10; retries; retries--)
137  {
138  // Set up the urgwidget
139  try
140  {
141  if (ip_address != "")
142  {
143  urg_.reset(new urg_node::URGCWrapper(ip_address, ip_port,
144  publish_intensity, publish_multiecho, synchronize_time));
145  }
146  else
147  {
148  urg_.reset(new urg_node::URGCWrapper(serial_baud, serial_port,
149  publish_intensity, publish_multiecho, synchronize_time));
150  }
151  std::string device_id = urg_->getDeviceID();
152  if (verbose)
153  {
154  if (ip_address != "")
155  {
156  printf("Device at %s:%i has ID ", ip_address.c_str(), ip_port);
157  }
158  else
159  {
160  printf("Device at %s has ID ", serial_port.c_str());
161  }
162  }
163  // Print this in either mode
164  fflush(NULL); // Clear whatever we aren't printing
165  dup2(save_stdout, STDOUT_FILENO); // Restore std::out
166  printf("%s\n", device_id.c_str());
167  return 0;
168  }
169  catch (std::runtime_error& e)
170  {
171  printf("getID failed: %s\n", e.what());
172  }
173  ros::Duration(1.0).sleep();
174  }
175 
176  if (verbose)
177  {
178  printf("getID failed for 10 seconds. Giving up.\n");
179  if (ip_address != "")
180  {
181  printf("Device at %s:%i\n", ip_address.c_str(), ip_port);
182  }
183  else
184  {
185  printf("Device at %s\n", serial_port.c_str());
186  }
187  }
188  return 1;
189 }
bool sleep() const
static void init()
std::vector< std::string > & split(const std::string &s, char delim, std::vector< std::string > &elems)
Definition: getID.cpp:54
int main(int argc, char **argv)
Definition: getID.cpp:74


urg_node
Author(s): Chad Rockey , Mike O'Driscoll
autogenerated on Wed Oct 28 2020 03:39:16