文章目录
1.C++编写Config类
1.1Config的头文件
//Config.h
#pragma once
#include <string>
#include <map>
#include <iostream>
#include <fstream>
#include <sstream>
/*
* \brief Generic configuration Class
*
*/
class Config {
// Data
protected:
std::string m_Delimiter; //!< separator between key and value
std::string m_Comment; //!< separator between value and comments
std::map<std::string,std::string> m_Contents; //!< extracted keys and values
typedef std::map<std::string,std::string>::iterator mapi;
typedef std::map<std::string,std::string>::const_iterator mapci;
// Methods
public:
Config( std::string filename,std::string delimiter = "=",std::string comment = "#" );
Config();
template<class T> T Read( const std::string& in_key ) const; //!<Search for key and read value or optional default value, call as read<T>
template<class T> T Read( const std::string& in_key, const T& in_value ) const;
template<class T> bool ReadInto( T& out_var, const std::string& in_key ) const;
template<class T>
bool ReadInto( T& out_var, const std::string& in_key, const T& in_value ) const;
bool FileExist(std::string filename);
void ReadFile(std::string filename,std::string delimiter = "=",std::string comment = "#" );
// Check whether key exists in configuration
bool KeyExists( const std::string& in_key ) const;
// Modify keys and values
template<class T> void Add( const std::string& in_key, const T& in_value );
void Remove( const std::string& in_key );
// Check or change configuration syntax
std::string GetDelimiter() const { return m_Delimiter; }
std::string GetComment() const { return m_Comment; }
std::string SetDelimiter( const std::string& in_s )
{ std::string old = m_Delimiter; m_Delimiter = in_s; return old; }
std::string SetComment( const std::string& in_s )
{ std::string old = m_Comment; m_Comment = in_s; return old; }
// Write or read configuration
friend std::ostream& operator<<( std::ostream& os, const Config& cf );
friend std::istream& operator>>( std::istream& is, Config& cf );
protected:
template<class T> static std::string T_as_string( const T& t );
template<class T> static T string_as_T( const std::string& s );
static void Trim( std::string& inout_s );
// Exception types
public:
struct File_not_found {
std::string filename;
File_not_found( const std::string& filename_ = std::string() )
: filename(filename_) {} };
struct Key_not_found { // thrown only by T read(key) variant of read()
std::string key;
Key_not_found( const std::string& key_ = std::string() )
: key(key_) {} };
};
/* static */
template<class T>
std::string Config::T_as_string( const T& t )
{
// Convert from a T to a string
// Type T must support << operator
std::ostringstream ost;
ost << t;
return ost.str();
}
/* static */
template<class T>
T Config::string_as_T( const std::string& s )
{
// Convert from a string to a T
// Type T must support >> operator
T t;
std::istringstream ist(s);
ist >> t;
return t;
}
/* static */
template<>
inline std::string Config::string_as_T<std::string>( const std::string& s )
{
// Convert from a string to a string
// In other words, do nothing
return s;
}
/* static */
template<>
inline bool Config::string_as_T<bool>( const std::string& s )
{
// Convert from a string to a bool
// Interpret "false", "F", "no", "n", "0" as false
// Interpret "true", "T", "yes", "y", "1", "-1", or anything else as true
bool b = true;
std::string sup = s;
for( std::string::iterator p = sup.begin(); p != sup.end(); ++p )
*p = toupper(*p); // make string all caps
if( sup==std::string("FALSE") || sup==std::string("F") ||
sup==std::string("NO") || sup==std::string("N") ||
sup==std::string("0") || sup==std::string("NONE") )
b = false;
return b;
}
template<class T>
T Config::Read( const std::string& key ) const
{
// Read the value corresponding to key
mapci p = m_Contents.find(key);
if( p == m_Contents.end() ) throw Key_not_found(key);
return string_as_T<T>( p->second );
}
template<class T>
T Config::Read( const std::string& key, const T& value ) const
{
// Return the value corresponding to key or given default value
// if key is not found
mapci p = m_Contents.find(key);
if( p == m_Contents.end() ) return value;
return string_as_T<T>( p->second );
}
template<class T>
bool Config::ReadInto( T& var, const std::string& key ) const
{
// Get the value corresponding to key and store in var
// Return true if key is found
// Otherwise leave var untouched
mapci p = m_Contents.find(key);
bool found = ( p != m_Contents.end() );
if( found ) var = string_as_T<T>( p->second );
return found;
}
template<class T>
bool Config::ReadInto( T& var, const std::string& key, const T& value ) const
{
// Get the value corresponding to key and store in var
// Return true if key is found
// Otherwise set var to given default
mapci p = m_Contents.find(key);
bool found = ( p != m_Contents.end() );
if( found )
var = string_as_T<T>( p->second );
else
var = value;
return found;
}
template<class T>
void Config::Add( const std::string& in_key, const T& value )
{
// Add a key with given value
std::string v = T_as_string( value );
std::string key=in_key;
Trim(key);
Trim(v);
m_Contents[key] = v;
return;
}
1.2Config的cpp文件
// Config.cpp
#include "Config.h"
using namespace std;
Config::Config( string filename, string delimiter,
string comment )
: m_Delimiter(delimiter), m_Comment(comment)
{
// Construct a Config, getting keys and values from given file
std::ifstream in( filename.c_str() );
if( !in ) throw File_not_found( filename );
in >> (*this);
}
Config::Config()
: m_Delimiter( string(1,'=') ), m_Comment( string(1,'#') )
{
// Construct a Config without a file; empty
}
bool Config::KeyExists( const string& key ) const
{
// Indicate whether key is found
mapci p = m_Contents.find( key );
return ( p != m_Contents.end() );
}
/* static */
void Config::Trim( string& inout_s )
{
// Remove leading and trailing whitespace
static const char whitespace[] = " \n\t\v\r\f";
inout_s.erase( 0, inout_s.find_first_not_of(whitespace) );
inout_s.erase( inout_s.find_last_not_of(whitespace) + 1U );
}
std::ostream& operator<<( std::ostream& os, const Config& cf )
{
// Save a Config to os
for( Config::mapci p = cf.m_Contents.begin();
p != cf.m_Contents.end();
++p )
{
os << p->first << " " << cf.m_Delimiter << " ";
os << p->second << std::endl;
}
return os;
}
void Config::Remove( const string& key )
{
// Remove key and its value
m_Contents.erase( m_Contents.find( key ) );
return;
}
std::istream& operator>>( std::istream& is, Config& cf )
{
// Load a Config from is
// Read in keys and values, keeping internal whitespace
typedef string::size_type pos;
const string& delim = cf.m_Delimiter; // separator
const string& comm = cf.m_Comment; // comment
const pos skip = delim.length(); // length of separator
string nextline = ""; // might need to read ahead to see where value ends
while( is || nextline.length() > 0 )
{
// Read an entire line at a time
string line;
if( nextline.length() > 0 )
{
line = nextline; // we read ahead; use it now
nextline = "";
}
else
{
std::getline( is, line );
}
// Ignore comments
line = line.substr( 0, line.find(comm) );
// Parse the line if it contains a delimiter
pos delimPos = line.find( delim );
if( delimPos < string::npos )
{
// Extract the key
string key = line.substr( 0, delimPos );
line.replace( 0, delimPos+skip, "" );
// See if value continues on the next line
// Stop at blank line, next line with a key, end of stream,
// or end of file sentry
bool terminate = false;
while( !terminate && is )
{
std::getline( is, nextline );
terminate = true;
string nlcopy = nextline;
Config::Trim(nlcopy);
if( nlcopy == "" ) continue;
nextline = nextline.substr( 0, nextline.find(comm) );
if( nextline.find(delim) != string::npos )
continue;
nlcopy = nextline;
Config::Trim(nlcopy);
if( nlcopy != "" ) line += "\n";
line += nextline;
terminate = false;
}
// Store key and value
Config::Trim(key);
Config::Trim(line);
cf.m_Contents[key] = line; // overwrites if key is repeated
}
}
return is;
}
bool Config::FileExist(std::string filename)
{
bool exist= false;
std::ifstream in( filename.c_str() );
if( in )
exist = true;
return exist;
}
void Config::ReadFile( string filename, string delimiter,
string comment )
{
m_Delimiter = delimiter;
m_Comment = comment;
std::ifstream in( filename.c_str() );
if( !in ) throw File_not_found( filename );
in >> (*this);
}
1.3调用Config的类读取txt
//main.cpp
#include "Config.h"
int main()
{
int port;
std::string ipAddress;
std::string username;
std::string password;
const char ConfigFile[]= "..//src//config.txt";
Config configSettings(ConfigFile);
port = configSettings.Read("port", 0);
ipAddress = configSettings.Read("ipAddress", ipAddress);
username = configSettings.Read("username", username);
password = configSettings.Read("password", password);
std::cout<<"port:"<<port<<std::endl;
std::cout<<"ipAddress:"<<ipAddress<<std::endl;
std::cout<<"username:"<<username<<std::endl;
std::cout<<"password:"<<password<<std::endl;
//
float level = configSettings.Read("level", level);
std::cout<<"level:"<<level<<std::endl;
return 0;
}
2.ROS中的Config动态参数
2.1动态参数编写
在ROS系统中实现某个功能时,时常需要外部动态调整参数。例如控制中PID参数的调节,或者需要查看机器人在不同参数下的性能表现,此时外部参数的动态调节显得极其便利。在ROS中可以通过动态参数功能来实现。接下讲解ROS中动态参数的应用。
msf_core中的imu的噪声和漂移参数设置文件如下:
1.利用python语言编写.cfg文件,设置动态参数
#! /usr/bin/env python
PACKAGE='msf_core'
import roslib; roslib.load_manifest(PACKAGE)
#from driver_base.msg import SensorLevels
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()#以上代码初始化ROS并导入参数生成器
# Name Type Reconfiguration level Description Default Min Max
gen.add("core_fixed_bias", bool_t, 0, "fix biases", False)
gen.add("core_noise_acc", double_t, 0, "accelerations noise spectral density (nsd) [m/s^2/sqrt(Hz)]", 6.93e-4, 1.0e-5, 1.0e-3)
gen.add("core_noise_accbias", double_t, 0, "acceleration biases random walk [m/s^3/sqrt(Hz)]", 2e-8, 1.0e-8, 1.0e-3)
gen.add("core_noise_gyr", double_t, 0, "gyros noise spectral density (nsd) [rad/s/sqrt(Hz)]", 1.39e-5, 1.0e-6, 1.0e-3)
gen.add("core_noise_gyrbias", double_t, 0, "gyro biases random walk [rad/s^2/sqrt(Hz)]", 3e-6, 1.0e-7, 1.0e-3)
exit(gen.generate(PACKAGE, "Config", "MSF_Core"))
#以上代码为加入不同的参数。其中gen.add(...)格式如下:
# gen.add(name, type, level, description, default, min, max)
# name: 参数的名称
# type: 参数类型
#level:一个传递给回调的位掩码
#description: 一个描述参数
#default: 节点启动的初始值
# min: 参数最小值
# max: 参数最大值
2.2动态参数生成
2. 编辑CMakeLists.txt文件
加入以下代码
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
message_generation
dynamic_reconfigure#加入所需要的包
)
generate_dynamic_reconfigure_options(cfg/MSF_Core.cfg)#生成动态参数
2.3调用动态参数
在MSF_SensorManagerROS.h文件中调用了该动态参数
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <msf_core/MSF_CoreConfig.h>//消息类型
typedef dynamic_reconfigure::Server<msf_core::MSF_CoreConfig> ReconfigureServer;//重命名
#在自定义的结构体MSF_SensorManagerROS中进行调用
struct MSF_SensorManagerROS : public msf_core::MSF_SensorManager<EKFState_T> {
....
MSF_SensorManagerROS(ros::NodeHandle pnh = ros::NodeHandle("~core")) {//构造函数
reconfServer_ = new ReconfigureServer(pnh);// 初始化服务器,忽略config的配置
ReconfigureServer::CallbackType f = boost::bind(
&MSF_SensorManagerROS::Config, this, _1, _2);//Config将会输出参数的新值
reconfServer_->setCallback(f);//向服务器发送Config函数,进行回调触发
//当服务器得到重新配置请求,一般运行$rosrun rqt_reconfigure rqt_reconfigure
//会调用回调函数f
}
....
//回调函数实现,把Gui的参数从新赋值个config_(结构体中的私有成员变量)
void Config(msf_core::MSF_CoreConfig &config, uint32_t level) {
config_ = config;//私有成员变量msf_core::MSF_CoreConfig config_; ///< Dynamic reconfigure config.
CoreConfigCallback(config, level);
}
.......
}
$rosrun rqt_reconfigure rqt_reconfigure
运行显示