microstrain_3dmgx2_imu: microstrain_3dmgx2_imu: get

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00039 #include <string>
00040 #include <boost/format.hpp>
00041 
00042 #include "ros/console.h"
00043 #include "microstrain_3dmgx2_imu/3dmgx2.h"
00044 
00045 
00046 std::string getID(microstrain_3dmgx2_imu::IMU &imu)
00047 {
00048   char dev_name[17];
00049   char dev_model_num[17];
00050   char dev_serial_num[17];
00051   char dev_opt[17];
00052   imu.getDeviceIdentifierString(microstrain_3dmgx2_imu::IMU::ID_DEVICE_NAME, dev_name);
00053   imu.getDeviceIdentifierString(microstrain_3dmgx2_imu::IMU::ID_MODEL_NUMBER, dev_model_num);
00054   imu.getDeviceIdentifierString(microstrain_3dmgx2_imu::IMU::ID_SERIAL_NUMBER, dev_serial_num);
00055   imu.getDeviceIdentifierString(microstrain_3dmgx2_imu::IMU::ID_DEVICE_OPTIONS, dev_opt);
00056   
00057   char *dev_name_ptr = dev_name;
00058   char *dev_model_num_ptr = dev_model_num;
00059   char *dev_serial_num_ptr = dev_serial_num;
00060   
00061   while (*dev_name_ptr == ' ')
00062     dev_name_ptr++;
00063   while (*dev_model_num_ptr == ' ')
00064     dev_model_num_ptr++;
00065   while (*dev_serial_num_ptr == ' ')
00066     dev_serial_num_ptr++;
00067   
00068   return (boost::format("%s_%s-%s")%dev_name_ptr%dev_model_num_ptr%dev_serial_num_ptr).str();
00069 }
00070 
00071 int main(int argc, char **argv)
00072 {
00073   if (argc < 2 || argc > 3)
00074   {
00075     fprintf(stderr, "usage: get_id /dev/ttyUSB? [quiet]\nOutputs the device ID of an IMU at that port. Add a second argument for script friendly output.\n");
00076     return 1;
00077   }
00078 
00079   bool verbose = (argc == 2);
00080   if (!verbose)
00081   {
00082     // In quiet mode we want to turn off logging levels that go to stdout.
00083     log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
00084     my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Error]);
00085   }
00086 
00087   microstrain_3dmgx2_imu::IMU imu;
00088 
00089   try
00090   {
00091     imu.openPort(argv[1]);
00092   }
00093   catch (microstrain_3dmgx2_imu::Exception& e) 
00094   {
00095     fprintf(stderr, "Unable to open IMU at port %s. IMU may be disconnected.\n%s", argv[1], e.what());
00096     return 1;
00097   }
00098 
00099   imu.initTime(0.0);
00100   
00101   std::string id = getID(imu);
00102 
00103   if (verbose)
00104     fprintf(stdout, "IMU Device at port %s has ID: ", argv[1]);
00105   fprintf(stdout, "%s\n", id.c_str());
00106 
00107   try
00108   {
00109     imu.closePort();
00110   } 
00111   catch (microstrain_3dmgx2_imu::Exception& e) 
00112   {
00113     fprintf(stderr, "Exception thrown while stopping IMU.\n%s", e.what());
00114     return 1;
00115   }
00116   
00117   return 0;
00118 }
get_id.cpp
