yocs_velocity_smoother速度平滑包的动态参数设置

ROS功能:动态重配置实用程序;浅显说就是在程序外部动态改变变量值,可以不用重新编译程序,就是说可以快速调参,其中需要用参数服务器,服务和主题.

配置文件保存在和src同级的文件夹cfg中,yocs_velocity_smoother的配置文件../yocs_velocity_smoother/cfg/params.cfg

#!/usr/bin/env python

PACKAGE = "yocs_velocity_smoother"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("speed_lim_v", double_t, 0, "Maximum linear velocity", 1.0, 0.0, 10.0)
gen.add("speed_lim_w", double_t, 0, "Maximum angular velocity", 5.0, 0.0, 10.0)

gen.add("accel_lim_v", double_t, 0, "Maximum linear acceleration", 0.5, 0.0, 10.0)
gen.add("accel_lim_w", double_t, 0, "Maximum angular acceleration", 2.5, 0.0, 10.0)

gen.add("decel_factor", double_t, 0, "Deceleration to acceleration ratio", 1.0, 0.0, 10.0)

# Second arg is node name it will run in (doc purposes only), third is generated filename prefix
exit(gen.generate(PACKAGE, "velocity_smoother_configure", "params"))

一.代码初始化生成器

#!/usr/bin/env python

PACKAGE = "yocs_velocity_smoother"

from dynamic_reconfigure.parameter_generator_catkin import *

二.不同参数类型默认值,描述,范围等的设置

gen = ParameterGenerator()

gen.add("speed_lim_v", double_t, 0, "Maximum linear velocity", 1.0, 0.0, 10.0)
gen.add("speed_lim_w", double_t, 0, "Maximum angular velocity", 5.0, 0.0, 10.0)

gen.add("accel_lim_v", double_t, 0, "Maximum linear acceleration", 0.5, 0.0, 10.0)
gen.add("accel_lim_w", double_t, 0, "Maximum angular acceleration", 2.5, 0.0, 10.0)

gen.add("decel_factor", double_t, 0, "Deceleration to acceleration ratio", 1.0, 0.0, 10.0)

模板如下:

gen.add(name, type, level, description, default, min, max)

name: 参数名称

type: 参数值类型

level: 传递给回调的位掩码

description: 节点启动时的默认值

min: 参数最小值

max: 参数最大值

三.生成必要的文件并退出

exit(gen.generate(PACKAGE, "velocity_smoother_configure", "params"))

四.CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS roscpp pluginlib nodelet ecl_threads dynamic_reconfigure geometry_msgs nav_msgs)

generate_dynamic_reconfigure_options(cfg/params.cfg)
.....
add_dependencies(${PROJECT_NAME}_nodelet ${PROJECT_NAME}_gencfg)

五. .h和.cpp文件的代码段也就是类似于在/src的main主函数节点

/xxx.h
dynamic_reconfigure::Server<yocs_velocity_smoother::paramsConfig> *             dynamic_reconfigure_server;
dynamic_reconfigure::Server<yocs_velocity_smoother::paramsConfig>::CallbackType dynamic_reconfigure_callback;
void reconfigCB(yocs_velocity_smoother::paramsConfig &config, uint32_t unused_level);

/xxx.cpp

#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <yocs_velocity_smoother/paramsConfig.h>

void VelocitySmoother::reconfigCB(yocs_velocity_smoother::paramsConfig &config, uint32_t level)
{
  ROS_INFO("Reconfigure request : %f %f %f %f %f",
           config.speed_lim_v, config.speed_lim_w, config.accel_lim_v, config.accel_lim_w, config.decel_factor);

  speed_lim_v  = config.speed_lim_v;
  speed_lim_w  = config.speed_lim_w;
  accel_lim_v  = config.accel_lim_v;
  accel_lim_w  = config.accel_lim_w;
  decel_factor = config.decel_factor;
  decel_lim_v  = decel_factor*accel_lim_v;
  decel_lim_w  = decel_factor*accel_lim_w;
}

....
bool VelocitySmoother::init(ros::NodeHandle& nh)
{
 // Dynamic Reconfigure
  dynamic_reconfigure_callback = boost::bind(&VelocitySmoother::reconfigCB, this, _1, _2);

  dynamic_reconfigure_server = new dynamic_reconfigure::Server<yocs_velocity_smoother::paramsConfig>(nh);
  dynamic_reconfigure_server->setCallback(dynamic_reconfigure_callback);
...
}

头文件:ROS头文件,参数服务器以及创建的config头文件

reconfigCB回调函数的参数名称必须和params.cfg的参数名称一致.

  // Dynamic Reconfigure
  dynamic_reconfigure_callback = boost::bind(&VelocitySmoother::reconfigCB, this, _1, _2);

  dynamic_reconfigure_server = new dynamic_reconfigure::Server<yocs_velocity_smoother::paramsConfig>(nh);//初始化服务器
  dynamic_reconfigure_server->setCallback(dynamic_reconfigure_callback);

初始化服务器,向服务器发送 reconfigCB回调函数.当服务器得到重新配置请求时,会调用reconfigCB函数.

参考:<<ROS机器人高效编程>>