ros dynamic_reconfigure 动态参数配置实例

1. 创建cfg 配置文件

创建cfg/DobotControl.cfg 文件,内容如下:
 

#!/usr/bin/env python
PACKAGE = "dobot"

from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()

gen.add( "hight_to_grab", double_t, 0,"the height of go to grab object",-40.0, -300, 300)
gen.add( "case_one_x", double_t,  0,     "X>0 Error compensation",  -0.035,    -10,       10)
gen.add( "case_one_y", double_t,  0,     "X>0 Error compensation",  0.03,      -10,       10)
gen.add( "case_two_x", double_t,  0,     "X > 0 and < 0.06 Error compensation",  -0.01,      -10,       10)
gen.add( "case_two_y", double_t,  0,     "X > 0 and < 0.06 Error compensation",  0.04,      -10,       10)
gen.add( "case_three_x", double_t,  0,     "3 X < -0.07  Error compensation",  0.015,     -10,       10)
gen.add( "case_three_y", double_t,  0,     "3 X < -0.07  Error compensation",  0.033,      -10,       10)
gen.add( "case_four_x", double_t,  0,     "4 X < 0  Error compensation",   -0.005,      -10,       10)
gen.add( "case_four_y", double_t,  0,     "4 X < 0  Error compensation",   0.03,      -10,       10)

exit(gen.generate(PACKAGE, "dobot", "DobotControl"))

gen.add( "hight_to_grab", double_t, 0,"the height of go to grab object",-40.0, -300, 300)

name:参数名,使用字符串描述;
type:定义参数的类型,可以是int_t, double_t, str_t, 或者bool_t;
level:需要传入参数动态配置回调函数中的掩码,在回调函数中会修改所有参数的掩码,表示参数已经进行修改;
description:描述参数作用的字符串;
default:设置参数的默认值;
3min:可选,设置参数的最小值,对于字符串和布尔类型值不生效;
max:可选,设置参数的最大值,对于字符串和布尔类型值不生效;

#第二参数为dobot , 包名  
#第三参数为文件名,同时也是生成的头文件DobotControlConfig.h的前缀,
exit(gen.generate(PACKAGE, "dobot", "DobotControl"))

2. 在src 源码中使用

#include <dynamic_reconfigure/server.h>
#include "dobot/DobotControlConfig.h"

dynamic_reconfigure::Server<dobot::DobotControlConfig> server;
dynamic_reconfigure::Server<dobot::DobotControlConfig>::CallbackType f;

 f = boost::bind(&callback, _1);
 server.setCallback(f);

void callback(dobot::DobotControlConfig &config)
 {
    ROS_WARN("***Reconfigure Request: %lf %lf %lf %lf %lf %lf %lf %lf %lf",
                                    config.hight_to_grab,
                                    config.case_one_x,config.case_one_y,
                                    config.case_two_x,config.case_two_y,
                                    config.case_three_x,config.case_three_y,
                                    config.case_four_x,config.case_four_y
                                    );
    error_incorret_x_1=config.case_one_x;
    error_incorret_y_1=config.case_one_y;
    
    error_incorret_x_2=config.case_two_x;
    error_incorret_y_2=config.case_two_y;
    
    error_incorret_x_3=config.case_three_x;
    error_incorret_y_3=config.case_three_y;
    
    error_incorret_x_4=config.case_four_x;
    error_incorret_y_4=config.case_four_y;
        
 }

3. 在cmakeList.txt 和package.xml中添加编译信息]
 

find_package(catkin REQUIRED COMPONENTS
.....
  dynamic_reconfigure
)

 generate_dynamic_reconfigure_options(
   cfg/DobotControl.cfg
 )

catkin_package(
.........
 dynamic_reconfigure
)

在package.xml中 添加
 

<build_depend>dynamic_reconfigure</build_depend>

<run_depend>dynamic_reconfigure</run_depend>

 

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值