getID.cpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2009, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#include <urg_node/urg_c_wrapper.h>
#include <vector>
#include <string>

#include <fcntl.h>
#include <stdlib.h>
#include <stdio.h>
/* gcc defined unix */
#ifdef unix
#include <unistd.h>
#endif
#ifdef WIN32
#include <io.h>
#define pipe(X) _pipe(X, 4096, O_BINARY)
#define fileno _fileno
#define dup2 _dup2
#define read _read
#endif

std::vector<std::string> &split(const std::string &s, char delim, std::vector<std::string> &elems)
{
  std::stringstream ss(s);
  std::string item;
  while (std::getline(ss, item, delim))
  {
    elems.push_back(item);
  }
  return elems;
}


std::vector<std::string> split(const std::string &s, char delim)
{
  std::vector<std::string> elems;
  split(s, delim, elems);
  return elems;
}

int
main(int argc, char** argv)
{
  ros::Time::init();

  if (argc < 2 || argc > 3)
  {
    fprintf(stderr,
        "usage: getID /dev/ttyACM? [quiet]\nOutputs the device ID of a hokuyo at /dev/ttyACM? or IP address"
        " (specified as 192.168.1.6:10940). Add a second argument for script friendly output.\n");
    return 1;
  }

  bool verbose = (argc == 2);

  int save_stdout = dup(STDOUT_FILENO);

  if (!verbose)  // Block urg's prints
  {
    int fds[2];
    int res;
    char buf[256];
    int so;

    res = pipe(fds);
    assert(res == 0);

    so = fileno(stdout);
    // close stdout handle and make the writable part of fds the new stdout.
    res = dup2(fds[1], so);
    assert(res != -1);
  }


  bool publish_intensity = false;
  bool publish_multiecho = false;
  int serial_baud = 115200;
  int ip_port = 10940;
  std::string ip_address = "";
  std::string serial_port = "";

  std::vector<std::string> ip_split = split(argv[1], ':');
  if (ip_split.size() < 2)  // Not an IP address
  {
    serial_port = argv[1];
  }
  else if (ip_split.size() == 2)   // IP address formatted as IP:PORT
  {
    ip_address = ip_split[0];
    ip_port = atoi(ip_split[1].c_str());
  }
  else     // Invalid
  {
    if (verbose)
    {
      printf("getID failed due to invalid specifier.\n");
      return 1;
    }
  }

  boost::shared_ptr<urg_node::URGCWrapper> urg_;

  for (int retries = 10; retries; retries--)
  {
    // Set up the urgwidget
    try
    {
      if (ip_address != "")
      {
        urg_.reset(new urg_node::URGCWrapper(ip_address, ip_port, publish_intensity, publish_multiecho));
      }
      else
      {
        urg_.reset(new urg_node::URGCWrapper(serial_baud, serial_port, publish_intensity, publish_multiecho));
      }
      std::string device_id = urg_->getDeviceID();
      if (verbose)
      {
        if (ip_address != "")
        {
          printf("Device at %s:%i has ID ", ip_address.c_str(), ip_port);
        }
        else
        {
          printf("Device at %s has ID ", serial_port.c_str());
        }
      }
      // Print this in either mode
      fflush(NULL);  // Clear whatever we aren't printing
      dup2(save_stdout, STDOUT_FILENO);  // Restore std::out
      printf("%s\n", device_id.c_str());
      return 0;
    }
    catch (std::runtime_error& e)
    {
      printf("getID failed: %s\n", e.what());
    }
    ros::Duration(1.0).sleep();
  }

  if (verbose)
  {
    printf("getID failed for 10 seconds. Giving up.\n");
    if (ip_address != "")
    {
      printf("Device at %s:%i\n", ip_address.c_str(), ip_port);
    }
    else
    {
      printf("Device at %s\n", serial_port.c_str());
    }
  }
  return 1;
}