ros動的パラメータの永続性dynamic_reconfigure
記事ディレクトリ
1. 問題点
ノードの move_base の一部のパラメーターは、dynamic_reconfigure を通じて構成され、オンライン調整をサポートします。ここで、読み取りと変更を実装し、ファイルの永続性を有効にするために、インターフェース ノードのいくつかのパラメーターを公開する必要があります。
2. 分析
1) 動的パラメータを取得します。
A. 動的パラメータサーバーコールバック取得：dynamic_reconfigure::Server
注：取得される値は動的パラメータcfgファイルによって生成されるデフォルト値であり、要件を満たしていないパラメータの最新値ではありません。
B. クライアントを介した取得: 積極的に取得する必要があり、他の変更方法ではタイムリーに取得するのが不便です
。
C. パラメータ更新トピックを受信します:「/move_base/parameter_updates」
kint@kint:~$ rostopic info /move_base/parameter_updates
Type: dynamic_reconfigure/Config
Publishers:
* /move_base (http://kint:37227/)
Subscribers:
* /robot_manager (http://kint:44333/)
kint@kint:~/csj_project/RenBot_ws/install/share/service_center/maps$ rostopic echo /move_base/parameter_updates
bools:
-
name: "recovery_behavior_enabled"
value: True
ints:
-
name: "max_planning_retries"
value: -1
strs:
-
name: "base_global_planner"
value: "global_planner/GlobalPlanner"
-
name: "base_local_planner"
value: "global_planner/GlobalPlanner"
doubles:
-
name: "planner_frequency"
value: 0.0
groups:
-
name: "Default"
state: True
id: 0
parent: 0
---
move_base_param_sub_ = private_nh_.subscribe("/move_base/parameter_updates", 2, &RobotManager::getMoveBaseParamCallBack, this);
void RobotManager::getMoveBaseParamCallBack(const dynamic_reconfigure::Config::ConstPtr config)
{
receive_dynamic_param_count_++;
LOG(INFO) << "topic: getMoveBaseParamCallBack";
dynamic_reconfigure::Config tmp = *config;
move_base_config_.__fromMessage__(tmp);
LOG(INFO)<<"getMoveBaseParamCallBack count:"<<receive_dynamic_param_count_ <<" v:"<<move_base_config_.max_vel_x<<" w:"<<move_base_config_.max_vel_th;
robot_config_.max_linear_vel = move_base_config_.max_vel_x;
robot_config_.max_angle_vel = move_base_config_.max_vel_th;
saveUserConfig();
}
2) 動的パラメータを変更します。
動的パラメータクライアント構成パラメータ:dynamic_reconfigure::Client
move_base_param_client.setConfiguration(move_base_param_config); auto move_base_param_cb = [](const move_base::MoveBaseConfig &data) -> void { LOG(INFO)<<" Not run !";};
dynamic_reconfigure::Client<move_base::MoveBaseConfig> move_base_param_client("/move_base", move_base_param_cb);
move_base::MoveBaseConfig move_base_param_config = move_base_config_;
move_base_param_config.max_vel_x = (double)req.robot_config.max_linear_vel;
move_base_param_client.setConfiguration(move_base_param_config);
3) パラメータファイルの永続化
jsonファイルの保存
bool RobotManager::loadUserConfig()
{
std::string user_config_path = ros::package::getPath("service_center") + "/user_config.json";
LOG(INFO) << "Load user_config_path: " << user_config_path;
ifstream file(user_config_path, ios::binary);
if (!file.is_open())
{
LOG(WARNING) << "Can not open file: " << user_config_path;
return false;
}
Json::Reader reader;
Json::Value root;
common_msgs::RobotConfig default_user_config;
if (reader.parse(file, root))
{
bool is_load_success = false;
for (unsigned int i = 0; i < root["user_config"].size(); i++)
{
default_user_config.max_linear_vel = root["user_config"]["max_linear_vel"].asFloat();
default_user_config.max_angle_vel = root["user_config"]["max_angle_vel"].asFloat();
is_load_success = true;
}
if(is_load_success)
{
common_srvs::SetRobotConfig set_srv;
set_srv.request.robot_config = default_user_config;
bool set_success = setRobotConfigSrv(set_srv.request, set_srv.response);
if(!set_success)
{
LOG(WARNING)<<"user_config set Failed !";
}
}
else
{
LOG(WARNING)<<"user_config load Failed !";
}
}
else
{
LOG(WARNING)<<"user_config Json Parse Failed !";
file.close();
return false;
}
file.close();
return true;
}
bool RobotManager::saveUserConfig()
{
Json::Value config_value;
config_value["user_config"]["max_linear_vel"] = robot_config_.max_linear_vel;
config_value["user_config"]["max_angle_vel"] = robot_config_.max_angle_vel;
Json::StreamWriterBuilder builder;
builder["commentStyle"] = "None";
builder["indentation"] = " ";
std::string user_config_path = ros::package::getPath("service_center") + "/user_config.json";
LOG(INFO) << "save user_config_path: " << user_config_path;
std::unique_ptr<Json::StreamWriter> writer(builder.newStreamWriter());
std::ofstream outputFileStream(user_config_path);
writer->write(config_value, &outputFileStream);
return true;
}
3 パラメータ同期解析
動的パラメータの更新は /move_base/parameter_updates で公開され、永続パラメータはトピック コールバックに従って更新され、ファイルに保存されます。デフォルトのファイルはノードの構築時にロードされます。
4 動的再構成サーバー分析
/opt/ros/melodic/include/dynamic_reconfigure/server.hには、1 つのサービスと 2 つのポスターが含まれています
。
/opt/ros/melodic/include/dynamic_reconfigure/server.h 构造部分代码
namespace dynamic_reconfigure
{
/**
* Keeps track of the reconfigure callback function.
*/
template <class ConfigType>
class Server
{
public:
Server(const ros::NodeHandle &nh = ros::NodeHandle("~")) :
node_handle_(nh),
mutex_(own_mutex_),
own_mutex_warn_(true)
{
init();
}
Server(boost::recursive_mutex &mutex, const ros::NodeHandle &nh = ros::NodeHandle("~")) :
node_handle_(nh),
mutex_(mutex),
own_mutex_warn_(false)
{
init();
}
void init()
{
//Grab copys of the data from the config files. These are declared in the generated config file.
min_ = ConfigType::__getMin__();
max_ = ConfigType::__getMax__();
default_ = ConfigType::__getDefault__();
boost::recursive_mutex::scoped_lock lock(mutex_);
set_service_ = node_handle_.advertiseService("set_parameters",
&Server<ConfigType>::setConfigCallback, this);
descr_pub_ = node_handle_.advertise<dynamic_reconfigure::ConfigDescription>("parameter_descriptions", 1, true);
descr_pub_.publish(ConfigType::__getDescriptionMessage__());
update_pub_ = node_handle_.advertise<dynamic_reconfigure::Config>("parameter_updates", 1, true);
ConfigType init_config = ConfigType::__getDefault__();
init_config.__fromServer__(node_handle_);
init_config.__clamp__();
updateConfigInternal(init_config);
}
}
5 Dynamic_reconfigure クライアント分析
/opt/ros/melodic/include/dynamic_reconfigure/client.h 分析では、そのコンストラクターを直接調べます。主に、serviceClient と 2 つのサブスクライブで構成されていることがわかります。
set_service_: set_parameters、dynamic_reconfigure::Reconfigure
config_sub_:parameter_updates、dynamic_reconfigure::Config
descr_sub_:parameter_descriptions、dynamic_reconfigure::ConfigDescription
/opt/ros/melodic/include/dynamic_reconfigure/client.h 构造部分代码
namespace dynamic_reconfigure {
template <class ConfigType>
class Client {
public:
/**
* @brief Client Constructs a statefull dynamic_reconfigure client
* @param name The full path of the dynamic_reconfigure::Server
* @param config_callback A callback that should be called when the server
* informs the clients of a successful reconfiguration
* @param description_callback A callback that should be called when the
* server infrorms the clients of the description of the reconfiguration
* parameters and groups
*/
Client(
const std::string& name,
const boost::function<void(const ConfigType&)> config_callback = 0,
const boost::function<void(const dynamic_reconfigure::ConfigDescription&)>
description_callback = 0)
: name_(name),
nh_(name),
received_configuration_(false),
received_description_(false),
config_callback_(config_callback),
description_callback_(description_callback) {
set_service_ =
nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_parameters");
config_sub_ =
nh_.subscribe("parameter_updates", 1,
&Client<ConfigType>::configurationCallback, this);
descr_sub_ = nh_.subscribe("parameter_descriptions", 1,
&Client<ConfigType>::descriptionCallback, this);
}
/**
* @brief Client Constructs a statefull dynamic_reconfigure client
* @param name The full path of the dynamic_reconfigure::Server
* @param nh The nodehandle to the full path of the Server (for nodelets)
* @param config_callback A callback that should be called when the server
* informs the clients of a successful reconfiguration
* @param description_callback A callback that should be called when the
* server infrorms the clients of the description of the reconfiguration
* parameters and groups
*/
Client(
const std::string& name, const ros::NodeHandle& nh,
const boost::function<void(const ConfigType&)> config_callback = 0,
const boost::function<void(const dynamic_reconfigure::ConfigDescription&)>
description_callback = 0)
: name_(name),
nh_(nh),
received_configuration_(false),
received_description_(false),
config_callback_(config_callback),
description_callback_(description_callback) {
set_service_ =
nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_parameters");
config_sub_ =
nh_.subscribe("parameter_updates", 1,
&Client<ConfigType>::configurationCallback, this);
descr_sub_ = nh_.subscribe("parameter_descriptions", 1,
&Client<ConfigType>::descriptionCallback, this);
}
‘’‘’‘’
}
6.dynamic_reconfigure client クライアント テスト コード (C++)
参考： https://github.com/ros/dynamic_reconfigure/blob/noetic-devel/test/test_client.cpp
#include <move_base/move_base.h>
#include "glog/logging.h"
#include <costmap_2d/InflationPluginConfig.h>
#include <dynamic_reconfigure/server.h>
#include <dynamic_reconfigure/client.h>
using namespace dynamic_reconfigure;
using namespace costmap_2d;
InflationPluginConfig CONFIG;
ConfigDescription DESCRIPTION;
void configurationCallback(const InflationPluginConfig& config) {
ROS_INFO( "configurationCallback happened: %d %f %f %d ",
config.enabled, config.cost_scaling_factor, config.inflation_radius, config.inflate_unknown);
CONFIG = config;
}
void descriptionCallback(const ConfigDescription& description) {
ROS_INFO("Received description");
}
bool test()
{
std::cout<<"test"<<std::endl;
{
bool ret = ros::service::waitForService("/move_base/local_costmap/inflated_layer/set_parameters", 5);
if(!ret)
{
LOG(WARNING) << "wait set_parameters failed!";
return false;
}
}
Client<InflationPluginConfig> client("/move_base/local_costmap/inflated_layer", &configurationCallback, &descriptionCallback);
InflationPluginConfig CONFIG2;
client.getCurrentConfiguration(CONFIG2);
ROS_INFO( "CONFIG2: %d %f %f %d ",
CONFIG2.enabled, CONFIG2.cost_scaling_factor, CONFIG2.inflation_radius, CONFIG2.inflate_unknown);
ROS_INFO("Setting configuration");
Client<InflationPluginConfig> set_client("/move_base/local_costmap/inflated_layer", &configurationCallback, &descriptionCallback);
InflationPluginConfig CONFIG3 = CONFIG2;
CONFIG3.enabled = false;
CONFIG3.cost_scaling_factor = 0.3;
CONFIG3.inflation_radius = 0.3;
ROS_INFO( "CONFIG3: %d %f %f %d ",
CONFIG3.enabled, CONFIG3.cost_scaling_factor, CONFIG3.inflation_radius, CONFIG3.inflate_unknown);
set_client.setConfiguration(CONFIG3);
}
void SignalHandle(const char* data, int size)
{
std::ofstream fs("/tmp/nav_log/glog_dump.log",std::ios::app);
std::string str = std::string(data,size);
fs<<str;
fs.close();
LOG(ERROR)<<str;
}
int main(int argc, char** argv){
ros::init(argc, argv, "dynamic_reconfigure_client_test");
//ros::start();
ros::AsyncSpinner spinner(2);
spinner.start();
sleep(1);
test();
sleep(1);
ros::shutdown();
return(0);
}
また
#include <move_base/move_base.h>
#include "glog/logging.h"
#include <costmap_2d/InflationPluginConfig.h>
#include <dynamic_reconfigure/server.h>
#include <dynamic_reconfigure/client.h>
#include <thread>
using namespace dynamic_reconfigure;
using namespace costmap_2d;
InflationPluginConfig CONFIG;
ConfigDescription DESCRIPTION;
void configurationCallback(const InflationPluginConfig& config) {
ROS_INFO( "configurationCallback happened: %d %f %f %d ",
config.enabled, config.cost_scaling_factor, config.inflation_radius, config.inflate_unknown);
CONFIG = config;
}
void descriptionCallback(const ConfigDescription& description) {
ROS_INFO("Received description");
}
bool test()
{
std::cout<<"test"<<std::endl;
{
bool ret = ros::service::waitForService("/move_base/local_costmap/inflated_layer/set_parameters", 5);
if(!ret)
{
LOG(WARNING) << "wait set_parameters failed!";
return false;
}
std::cout<<"ret:"<<ret<<std::endl;
}
Client<InflationPluginConfig> client("/move_base/local_costmap/inflated_layer", &configurationCallback, &descriptionCallback);
InflationPluginConfig CONFIG2;
client.getCurrentConfiguration(CONFIG2);
ROS_INFO( "CONFIG2: %d %f %f %d ",
CONFIG2.enabled, CONFIG2.cost_scaling_factor, CONFIG2.inflation_radius, CONFIG2.inflate_unknown);
ROS_INFO("Setting configuration");
Client<InflationPluginConfig> set_client("/move_base/local_costmap/inflated_layer", &configurationCallback, &descriptionCallback);
InflationPluginConfig CONFIG3 = CONFIG2;
CONFIG3.enabled = false;
CONFIG3.cost_scaling_factor = 0.3;
CONFIG3.inflation_radius = 0.3;
ROS_INFO( "CONFIG3: %d %f %f %d ",
CONFIG3.enabled, CONFIG3.cost_scaling_factor, CONFIG3.inflation_radius, CONFIG3.inflate_unknown);
set_client.setConfiguration(CONFIG3);
}
void SignalHandle(const char* data, int size)
{
std::ofstream fs("/tmp/nav_log/glog_dump.log",std::ios::app);
std::string str = std::string(data,size);
fs<<str;
fs.close();
LOG(ERROR)<<str;
}
int main(int argc, char** argv){
ros::init(argc, argv, "dynamic_reconfigure_client_test");
ros::start();
// ros::AsyncSpinner spinner(2);
// spinner.start();
std::thread th(test);
th.detach();
ros::spin();
return(0);
}
**解決すべき問題:** マルチスレッド メソッドは ros::AsyncSpinner のみ使用でき、シングルスレッド メソッドは常に
「設定を待機中...」になります。? ?
参考
1.https://stackoverflow.com/questions/4289986/jsoncpp-writing-to-files
2.https://github.com/ros/dynamic_reconfigure
3.https://github.com/WelinLee/ROS_dynamic_reconfig
4.https://github.com/open-source-parsers/jsoncpp/tree/master/example