ros中move_group的参数动态设置

最新推荐文章于 2024-04-26 10:49:02 发布

weixin_34117211

最新推荐文章于 2024-04-26 10:49:02 发布

阅读量615

点赞数

文章标签： c/c++ python

原文链接：https://my.oschina.net/itfanr/blog/1931939

版权

为什么80%的码农都做不了架构师？>>>

1、写一个配置文件 cfg/ElfinBasicAPIDynamicReconfigure.cfg

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

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("velocity_scaling", double_t, 0, "the max velocity scaling", 0.4,  0.01, 1.0)

exit(gen.generate(PACKAGE, PACKAGE, "ElfinBasicAPIDynamicReconfigure"))

2、cmake自动产生 ElfinBasicAPIDynamicReconfigureConfig.h头文件

find_package(catkin REQUIRED COMPONENTS
 
  dynamic_reconfigure
)

generate_dynamic_reconfigure_options(
  cfg/ElfinBasicAPIDynamicReconfigure.cfg
)

3、使用dynamic_reconfigure::Server

#include <elfin_basic_api/ElfinBasicAPIDynamicReconfigureConfig.h> //引入头文件

//自定义类中加入成员变量：
dynamic_reconfigure::Server<ElfinBasicAPIDynamicReconfigureConfig> dynamic_reconfigure_server_;
//绑定回调函数
dynamic_reconfigure_server_.setCallback(boost::bind(&ElfinBasicAPI::dynamicReconfigureCallback, this, _1, _2));

void ElfinBasicAPI::dynamicReconfigureCallback(ElfinBasicAPIDynamicReconfigureConfig &config, uint32_t level)
{
    setVelocityScaling(config.velocity_scaling);
}

void ElfinBasicAPI::setVelocityScaling(double data)
{
    velocity_scaling_=data;
    teleop_api_->setVelocityScaling(velocity_scaling_);
}

于是，elfin_basic_api自动启动了serice：

/elfin_basic_api/set_parameters

和topic：

/elfin_basic_api/parameter_descriptions
/elfin_basic_api/parameter_updates

4、move_group动态修改参数

void ElfinTeleopAPI::setVelocityScaling(double data)
{
    velocity_scaling_=data;
    joint_speed_=joint_speed_default_*velocity_scaling_;
    cart_duration_=cart_duration_default_/velocity_scaling_;
    group_->setMaxVelocityScalingFactor(velocity_scaling_);
}

python脚本修改参数

设置elfin_basic_api相关的动态参数，例如: velocity scaling  
example: set_parameters() in elfin_robot_bringup/script/set_velocity_scaling.py

python源码：

# author: Cong Liu
import rospy
from dynamic_reconfigure.srv import Reconfigure, ReconfigureRequest
from dynamic_reconfigure.msg import DoubleParameter, Config

class SetVelocityScaling(object):
    def __init__(self):
        self.request=ReconfigureRequest()
        self.velocity_scaling_goal=0.6
        self.elfin_basic_api_ns='elfin_basic_api/'
        self.set_parameters_client=rospy.ServiceProxy(self.elfin_basic_api_ns+'set_parameters',
                                                      Reconfigure)
    
    def set_parameters(self):
        config_empty=Config()
        
        velocity_scaling_param_tmp=DoubleParameter()
        velocity_scaling_param_tmp.name='velocity_scaling'
        velocity_scaling_param_tmp.value=self.velocity_scaling_goal
        self.request.config.doubles.append(velocity_scaling_param_tmp)
        self.set_parameters_client.call(self.request)
        self.request.config=config_empty
    
if __name__ == "__main__":
    rospy.init_node('set_velocity_scaling', anonymous=True)
    svc=SetVelocityScaling()
    svc.set_parameters()
    rospy.spin()

参考： 1、https://github.com/hans-robot/elfin_robot

转载于:https://my.oschina.net/itfanr/blog/1931939