参考
ipa 覆盖算法源码
背景
工作中需要在 C++ 程序运行过程中,根据场景开关 costmap2d 的障碍物感知图层以及膨胀图层。在 yaml 启动参数配置中加载障碍物层插件和膨胀层插件,但是关闭它们的更新,大概如下:
global_costmap:
global_frame: map
robot_base_frame: base_footprint
...
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
static_layer:
map_topic: /static_map
obstacle_layer:
enabled: false
observation_sources: scan
scan:
topic: /base_scan
sensor_frame: base_laser_link
data_type: LaserScan
marking: true
clearing: true
inflation_layer:
enabled: false
inflation_radius: 1.0
cost_scaling_factor: 0.8
启动程序后,global_costmap 是没局部障碍物感知和障碍物膨胀层信息的:
![](https://i-blog.csdnimg.cn/blog_migrate/2de838c20b0d2924133f2d84b0745b92.png)
查看 global_costmap 的 obstacle_layer 的 enabled 参数:
![](https://i-blog.csdnimg.cn/blog_migrate/a85914beafb01d8f21950afa35145af1.png)
在程序运行起来后,当遇到需要开启障碍物层插件和膨胀层插件更新的场景时候,在 C++ 用 ros::param::set() 接口设置 obstacle_layer 和 inflation_layer 更新参数 enable 为 true。此时可以看到 global_costmap 还是没有出现局部障碍物的感知信息和膨胀层信息。
动态参数服务器实现原理
动态参数服务器设计成 CS 架构,客户端修改参数就是向服务器发送请求,服务器接收到请求之后,读取修改后的参数。
在代码上用 ros::param::set() 设置动态参数的方法就如同在命令行用 rosparam 的方法:
![](https://i-blog.csdnimg.cn/blog_migrate/6caf76af992ae71d845170e810989759.png)
这种直接在参数服务器修改参数的方式,并不能让 costmap2d 节点执行相关的动态参数服务器回调函数,自然就无法让 costmap2d 节点实时将动参数据更新到程序中。
动态参数修改方法
一般情况下,我们通过 rqt_reconfigure 工具提供的 GUI 图形界面来调试算法的动态参数,命令行指令如下:
$ rosrun rqt_reconfigure rqt_reconfigure
其他修改动态参数的方法,网上资料有点少,后来在 HokuyoLaser 项目的 wiki 找到 。゚(TヮT)゚。
下面是 wiki 的搬运:
命令行修改
首先列出系统中动态参数配置节点 <node_name>
rosrun dynamic_reconfigure dynparam list
修改动态参数配置节点 <node_name> 命名空间下的动参 <param> 值为 <value>
rosrun dynamic_reconfigure dynparam set /<node_name> <param> <value>
launch 修改
这里是 HokuyoLaser 项目的第一个例子。
<launch>
<node name="$(anon dynparam)" pkg="dynamic_reconfigure" type="dynparam" args="set_from_parameters hokuyo_node">
<param name="min_ang" type="double" value="-1.0" />
<param name="max_ang" type="double" value="1.0" />
</node>
</launch>
python 修改
导入动态参数的客户端库
import dynamic_reconfigure.client
创建节点
rospy.init_node('myconfig_py', anonymous=True)
创建动参配置客户端实例,<node_to_reconfigure> 是动态参数配置节点
client = dynamic_reconfigure.client.Client(<node_to_reconfigure>)
修改参数
params = { 'my_string_parameter' : 'value', 'my_int_parameter' : 5 }
config = client.update_configuration(params)
c++ 修改
- 通过 system 运行命令行来修改动参 :)
<cmd> 就是上面 <命令行修改> 的方法。
system("<cmd>");
e.g.
system("rosrun dynamic_reconfigure dynparam set_from_parameters camera_synchronizer_node narrow_stereo_trig_mode 3");
- 通过 DynamicReconfigureClient 动参客户端来修改。
这个方法是后来在测试 ipa 功能包时候发现的。但这个类却没有找到官方比较靠谱的介绍。源码如下:
DynamicReconfigureClient drc_exp(nh, "room_exploration_server/set_parameters", "room_exploration_server/parameter_updates");
drc_exp.setConfig("room_exploration_algorithm", 8);
drc_exp.setConfig("execute_path", false);