ros_env.hpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Aldebaran
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 
00019 #ifndef ROS_ENV_HPP
00020 #define ROS_ENV_HPP
00021 
00022 /*
00023 * ROS includes
00024 */
00025 #include <ros/ros.h>
00026 
00027 
00028 /*
00029 * ALDEBARAN includes
00030 */
00031 #include <qi/os.hpp>
00032 
00033 #include <stdlib.h>
00034 
00035 #include <boost/algorithm/string.hpp>
00036 
00037 namespace naoqi
00038 {
00039 namespace ros_env
00040 {
00041 
00046 static std::string getROSIP(std::string network_interface)
00047 {
00048   if (network_interface.empty())
00049     network_interface = "eth0";
00050 
00051   typedef std::map< std::string, std::vector<std::string> > Map_IP;
00052   Map_IP map_ip = static_cast<Map_IP>(qi::os::hostIPAddrs());
00053   if ( map_ip.find(network_interface) == map_ip.end() ) {
00054     std::cerr << "Could not find network interface named " << network_interface << ", possible interfaces are ... ";
00055     for (Map_IP::iterator it=map_ip.begin(); it!=map_ip.end(); ++it) std::cerr << it->first <<  " ";
00056     std::cerr << std::endl;
00057     exit(1);
00058   }
00059 
00060   static const std::string ip = map_ip[network_interface][0];
00061   return ip;
00062 }
00063 
00064 static std::string prefix = "";
00065 
00066 static void setPrefix( std::string s )
00067 {
00068   prefix = s;
00069   std::cout << "set prefix successfully to " << prefix << std::endl;
00070 }
00071 
00072 static std::string getPrefix()
00073 {
00074   return prefix;
00075 }
00076 
00077 static void setMasterURI( const std::string& uri, const std::string& network_interface )
00078 {
00079   if (ros::isInitialized() )
00080   {
00081     std::cout << "stopping ros init" << std::endl;
00082     ros::shutdown();
00083   }
00084 
00085   setenv("ROS_MASTER_URI", uri.c_str(), 1);
00086 
00087   std::string my_master = "__master="+uri;
00088   std::map< std::string, std::string > remap;
00089   remap["__master"] = uri;
00090   remap["__ip"] = ::naoqi::ros_env::getROSIP(network_interface);
00091   // init ros without a sigint-handler in order to shutdown correctly by naoqi
00092   const char* ns_env = std::getenv("ROS_NAMESPACE");
00093   ros::init( remap, (::naoqi::ros_env::getPrefix()), ros::init_options::NoSigintHandler );
00094   // to prevent shutdown based on no existing nodehandle
00095   ros::start();
00096 
00097   std::cout << "using master ip: " <<  ros::master::getURI() << std::endl;
00098 }
00099 
00100 static std::string getMasterURI( )
00101 {
00102   return getenv("ROS_MASTER_URI");
00103 }
00104 
00105 
00106 } // ros_env
00107 } // naoqi
00108 #endif


naoqi_driver
Author(s): Karsten Knese
autogenerated on Sun Sep 17 2017 02:57:14