StereoOdometryNode.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
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     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "ros/ros.h"
00029 #include "nodelet/loader.h"
00030 #include <rtabmap/utilite/ULogger.h>
00031 #include <rtabmap/core/Parameters.h>
00032 
00033 int main(int argc, char **argv)
00034 {
00035         ULogger::setType(ULogger::kTypeConsole);
00036         ULogger::setLevel(ULogger::kWarning);
00037         ros::init(argc, argv, "stereo_odometry");
00038 
00039         // process "--params" argument
00040         nodelet::V_string nargv;
00041         for(int i=1;i<argc;++i)
00042         {
00043                 if(strcmp(argv[i], "--params") == 0)
00044                 {
00045                         rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(true);
00046                         for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
00047                         {
00048                                 std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
00049                                 std::cout <<
00050                                                 str <<
00051                                                 std::setw(60 - str.size()) <<
00052                                                 " [" <<
00053                                                 rtabmap::Parameters::getDescription(iter->first).c_str() <<
00054                                                 "]" <<
00055                                                 std::endl;
00056                         }
00057                         ROS_WARN("Node will now exit after showing default odometry parameters because "
00058                                          "argument \"--params\" is detected!");
00059                         exit(0);
00060                 }
00061                 else if(strcmp(argv[i], "--udebug") == 0)
00062                 {
00063                         ULogger::setLevel(ULogger::kDebug);
00064                 }
00065                 else if(strcmp(argv[i], "--uinfo") == 0)
00066                 {
00067                         ULogger::setLevel(ULogger::kInfo);
00068                 }
00069                 nargv.push_back(argv[i]);
00070 
00071         }
00072 
00073         nodelet::Loader nodelet;
00074         nodelet::M_string remap(ros::names::getRemappings());
00075         std::string nodelet_name = ros::this_node::getName();
00076         nodelet.load(nodelet_name, "rtabmap_ros/stereo_odometry", remap, nargv);
00077         ros::spin();
00078         return 0;
00079 }


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Sun Jul 24 2016 03:49:08