ros中move_group的參數動態設置

一、寫一個配置文件 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"))

二、cmake自動產生 ElfinBasicAPIDynamicReconfigureConfig.h頭文件

find_package(catkin REQUIRED COMPONENTS
 
  dynamic_reconfigure
)

generate_dynamic_reconfigure_options(
  cfg/ElfinBasicAPIDynamicReconfigure.cfg
)

三、使用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

四、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()

參考： 一、https://github.com/hans-robot/elfin_robot