【 声明:版权所有,欢迎转载,请勿用于商业用途。 联系信箱:feixiaoxing @163.com】
在编写ros程序的过程中,很多时候需要不停修改程序的参数。比如说,我们有一个配置文件。在程序还没有运行之前,我们可以随意修改配置文件中的参数,这都没关系。但是程序运行过程中,如果我们还想继续调试参数,这就不太好处理了。好在ros工具已经替我们想到了这个,只要程序启动之后,就可以利用rosparam这个命令去修改参数。
这样,就在我们调试程序的时候,比如PID,就可以不用停止程序的执行,用rosparam命令修改参数后继续执行。添加和修改的这些参数在roscore重新执行后,都会恢复成原样。
1、ros参数的处理方式
ros获取和设置的方式很多,我们选择一个适合自己的方法就可以了,比如说,
ros::param::get()
ros::param::set()
2、准备测试代码book_param.cpp
#include "ros/ros.h"
#include <cstdlib>
using namespace std;
int main(int argc, char* argv[])
{
ros::init(argc, argv, "node_param");
if(argc != 2)
{
cout<< "Error command parameter! Please run command eg:\n" \
<< "rosrun beginner_tutorials book_param 1\n"\
<<"help information:\n"\
<<" 1 --- set param mode(ros::param::set()) \n"\
<<" 2 --- get param mode(ros::param::get())\n" \
<< endl;
return 1;
}
ros::NodeHandle nh;
int IntParam;
string StrParam;
bool isIntParam, isStrParam;
int flag = atoi(argv[1]);
switch(flag)
{
case 1:
ROS_INFO("set param mode(ros::param::set()):");
ros::param::set("IntParam", 1);
ros::param::set("StrParam", "stringdemo");
break;
case 2:
ROS_INFO("get param mode(ros::param::get():");
isIntParam = ros::param::get("IntParam", IntParam);
isStrParam = ros::param::get("StrParam", StrParam);
if(isIntParam)
{
ROS_INFO("The IntParam is:%d", IntParam);
}
else
{
ROS_INFO("Get IntParam fail!");
}
if(isStrParam)
{
ROS_INFO("The StrParam is:%s", StrParam.c_str());
}
else
{
ROS_INFO("Get StrParam fail!");
}
break;
default:
ROS_INFO("flag value is not in range: [1,2]");
break;
}
return 0;
}
整个代码就两个功能。一个是设置参数,一个是获取参数。
3、添加CMakelists.txt文件
add_executable(book_param src/book_param.cpp)
target_link_libraries(book_param ${catkin_LIBRARIES})
add_dependencies(book_param beginner_tutorials_generate_messages_cpp)
添加完内容之后,就可以在顶层输入catkin_make进行编译了。
4、开始测试
测试之前同样需要两个工作。第一,启动roscore;第二,source ./devel/setup.sh。两步都完成之后,首先可以执行rosrun beginner_tutorials book_param 1,
feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosrun beginner_tutorials book_param 1
[ INFO] [1695364339.899438043]: set param mode(ros::param::set()):
设置完参数之后,就可以获取参数,
feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosrun beginner_tutorials book_param 2
[ INFO] [1695364342.494899013]: get param mode(ros::param::get():
[ INFO] [1695364342.495595449]: The IntParam is:1
[ INFO] [1695364342.495661805]: The StrParam is:stringdemo
这个时候,就可以利用rosparam查看参数是不是存在于列表当中,
feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosparam list
/IntParam
/StrParam
/rosdistro
/roslaunch/uris/host_feixiaoxing_virtualbox__42111
/rosversion
/run_id
如果我们想要修改数据,也可以直接使用rosparam进行修改,
feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosparam get StrParam
stringdemo
feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosparam get IntParam
1
feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosparam set IntParam 2
feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosparam get IntParam
2
feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosparam set StrParam "china"
feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosparam get StrParam
china
这个时候,可以继续执行book_param,查看rosparam修改的参数是否可以被我们写的程序感知到,
feixiaoxing@feixiaoxing-VirtualBox:~/Desktop/catkin_ws$ rosrun beginner_tutorials book_param 2
[ INFO] [1695364423.079711352]: get param mode(ros::param::get():
[ INFO] [1695364423.080401191]: The IntParam is:2
[ INFO] [1695364423.080457110]: The StrParam is:china
至此,我们已经可以说掌握了基本的ros参数读取功能。