需求说明
在室内服务机器人的导航中,大多数不需要过低的目标点位姿容差,只在一些特殊点位需要低位姿容差比如充电站。对此dwa局部路径规划器中的xy_goal_tolerance、yaw_goal_tolerance在平常的巡航中可以相对较高,当机器人判断需要回充电站时,在程序中调节对应的参数,不需要人为的在rqt_reconfigure工具中调节。
目录
注意
- 以下cpp需要在CMakeilsts.txt中添加依赖dynamic_reconfigure
find_package(catkin REQUIRED
dynamic_reconfigure
)
- 查看你的动态参数节点名称:运行动态参数服务端节点
rosrun dynamic_reconfigure dynparam list
无回调函数
#include "ros/ros.h"
#include "dwa_local_planner/DWAPlannerConfig.h"
/*可以根据需要替换不同的Config头文件
#include "move_base/MoveBaseConfig.h"
#include "base_local_planner/BaseLocalPlannerConfig.h"
#include "global_planner/GlobalPlannerConfig.h"
#include "costmap_2d/Costmap2DConfig.h"
#include "costmap_2d/InflationPluginConfig.h"
...
*/
//dynamic_reconfigure客户端头文件
#include "dynamic_reconfigure/client.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "dwa_dynamic_reconfigure");
//声明定义一个动态调参客户端
//dynamic_reconfigure::Client<根据选择的Congfig头文件来填写>,
//client("根据动态参数节点名来填写")
dynamic_reconfigure::Client<dwa_local_planner::DWAPlannerConfig> client("/move_base/DWAPlannerROS");
//声明一个动态参数对象
dwa_local_planner::DWAPlannerConfig dwa_config;
ros::Rate loop_rate(5);
while (ros::ok())
{
//等待在1s,获取最新的配置
client.getCurrentConfiguration(dwa_config,ros::Duration(1));
if(dwa_config.xy_goal_tolerance > 0.1 && dwa_config.yaw_goal_tolerance >0.05) {
dwa_config.xy_goal_tolerance = 0.1;
dwa_config.yaw_goal_tolerance =0.05;
ROS_INFO("double: %f, double: %f", dwa_config.xy_goal_tolerance, dwa_config.yaw_goal_tolerance);
//设置配置参数 若没有通过getCurrentConfiguration先获取配置,则需要重新为每一个参数对象里的全部参数赋值
client.setConfiguration(dwa_config);
break;
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
有回调函数
#include "ros/ros.h"
#include "dwa_local_planner/DWAPlannerConfig.h"
/*可以根据需要替换不同的Config头文件
#include "move_base/MoveBaseConfig.h"
#include "base_local_planner/BaseLocalPlannerConfig.h"
#include "global_planner/GlobalPlannerConfig.h"
#include "costmap_2d/Costmap2DConfig.h"
#include "costmap_2d/InflationPluginConfig.h"
...
*/
//dynamic_reconfigure客户端头文件
#include "dynamic_reconfigure/client.h"
//当服务器通知客户端成功重新配置时的回调函数
void config_callback(const dwa_local_planner::DWAPlannerConfig &dwa_config)
{
ROS_INFO("a successful reconfiguration");
ROS_INFO("xy_goal_tolerance: %.2f, yaw_goal_tolerance: %.2f", dwa_config.xy_goal_tolerance, dwa_config.yaw_goal_tolerance);
}
//当服务器向客户端通知重新配置参数和组的描述时的回调函数
void description_callback(const dwa_local_planner::DWAPlannerConfig &dwa_config)
{
ROS_INFO("the reconfiguration parameters and groups");
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "dwa_dynamic_reconfigure");
//声明定义一个动态调参客户端
//dynamic_reconfigure::Client<根据选择的Congfig头文件来填写>,
//client("根据动态参数节点名来填写")
dynamic_reconfigure::Client<dwa_local_planner::DWAPlannerConfig> client("/move_base/DWAPlannerROS",config_callback,description_callback);
//声明一个动态参数对象
dwa_local_planner::DWAPlannerConfig dwa_config;
ros::Rate loop_rate(5);
while (ros::ok())
{
//等待在1s,获取最新的配置
client.getCurrentConfiguration(dwa_config,ros::Duration(1));
if(dwa_config.xy_goal_tolerance > 0.1 && dwa_config.yaw_goal_tolerance >0.05) {
dwa_config.xy_goal_tolerance = 0.1;
dwa_config.yaw_goal_tolerance =0.05;
ROS_INFO("xy_goal_tolerance: %.2f, yaw_goal_tolerance: %.2f", dwa_config.xy_goal_tolerance, dwa_config.yaw_goal_tolerance);
//设置配置参数 若没有通过getCurrentConfiguration先获取配置,则需要重新为每一个参数对象里的全部参数赋值
client.setConfiguration(dwa_config);
break;
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
通过类来加载
#include "ros/ros.h"
#include "std_msgs/Bool.h"
#include "dwa_local_planner/DWAPlannerConfig.h"
/*可以根据需要替换不同的Config头文件
#include "move_base/MoveBaseConfig.h"
#include "base_local_planner/BaseLocalPlannerConfig.h"
#include "global_planner/GlobalPlannerConfig.h"
#include "costmap_2d/Costmap2DConfig.h"
#include "costmap_2d/InflationPluginConfig.h"
...
*/
//dynamic_reconfigure客户端头文件
#include "dynamic_reconfigure/client.h"
class A
{
public:
A():client("/move_base/DWAPlannerROS",
boost::bind(&A::configurationCallback,this,_1)) {
sub_go_home=nh.subscribe("/gohome",100,&A::GoHomeCallback,this);
}
void GoHomeCallback(const std_msgs::Bool::ConstPtr &msg);
void configurationCallback(const dwa_local_planner::DWAPlannerConfig &config);
private:
ros::NodeHandle nh;
ros::Subscriber sub_go_home;
dwa_local_planner::DWAPlannerConfig dwa_config;
dynamic_reconfigure::Client<dwa_local_planner::DWAPlannerConfig> client;
};
void A::configurationCallback(const dwa_local_planner::DWAPlannerConfig &config)
{
ROS_INFO("a successful reconfiguration");
}
void A::GoHomeCallback(const std_msgs::Bool::ConstPtr &msg)
{
if(msg->data == true) {
client.getCurrentConfiguration(dwa_config,ros::Duration(1));
if(dwa_config.xy_goal_tolerance > 0.1 && dwa_config.yaw_goal_tolerance >0.05) {
dwa_config.xy_goal_tolerance = 0.1;
dwa_config.yaw_goal_tolerance =0.05;
ROS_INFO("xy_goal_tolerance: %.2f, yaw_goal_tolerance: %.2f", dwa_config.xy_goal_tolerance, dwa_config.yaw_goal_tolerance);
client.setConfiguration(dwa_config);
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "dwa_dynamic_reconfigure");
A oa;
ros::spin();
return 0;
}
欢迎大家一起交流