理解ROS的参数机制,编写C++操作参数样例
1 资料
(1)本人ROS高效入门系列博客的第二章的第2.7节:ROS高效入门第二章 – 基本概念和常用命令学习,基于小乌龟样例
(2)《机器人操作系统(ROS)浅析》[美] Jason M. O’Kane 著 肖军浩 译,第7章
(3)ros Tutorials 初级教程的第7节:ROS教程
本系列博客汇总:ROS高效入门系列。
2 正文
2.1 编程操作ros参数
(1)ros参数的原理和命令行操作参数的方式,可以参考本人ROS高效入门系列博客的第二章的第2.7节:ROS高效入门第二章 – 基本概念和常用命令学习,基于小乌龟样例,这里不再赘述,只把rosparam -h的截图放在这里:
(2)除了命令行操作参数,还有cpp编程操作ros参数,主要是下面的这对接口:
// parameter_name是参数名的字符串,input_value是输入参数,可以是std::string、bool、int 或 double 类型
void ros::param::set(parameter_name, input_value);
// output_value是输出参数,内部通过引用传递,类型也可以是std::string、bool、int 或 double 类型。获取成功,则返回True,否则为False
bool ros::param::get(parameter_name, output_value);
补充:rospy也提供了一对操作ros参数的接口
get_param(name, default=None)
样例:
my_param = rospy.get_param('~my_param', 'default_value')
set_param(name, value)
样例:
rospy.set_param('~my_param', 'new_value')
2.2 turtlesim_param样例
(1)该样例仍以ros的turtlesim为基础,演示cpp获取参数和设置参数。创建turtlesim_param软件包和文件
cd ~/catkin_ws/src/cpp
// 依赖ros的标准服务std_srvs
catkin_create_pkg turtlesim_param turtlesim geometry_msgs std_srvs rospy roscpp
mkdir launch
touch launch/start.launch src/set_bg.cpp src/pubvel_with_max.cpp
(2)编写set_bg.cpp,该节点设置turtlesim的背景颜色为黄色。
#include <ros/ros.h>
#include <std_srvs/Empty.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "set_bg_color");
ros::NodeHandle nh;
// 等待turtlesim_node和/clear服务起来
ros::service::waitForService("clear");
ros::param::set("turtlesim/background_r", 255);
ros::param::set("turtlesim/background_g", 255);
ros::param::set("turtlesim/background_b", 0);
// 设置背景颜色参数后,需要调用/clear服务才能生效,同命令行操作
ros::ServiceClient clearClient = nh.serviceClient<std_srvs::Empty>("clear");
std_srvs::Empty srv;
clearClient.call(srv);
return 0;
}
(3)编写pubvel_with_max.cpp,通过获取launch文件中设置的max_vel参数,发送特定的velocity信息。
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <stdlib.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "pub_velocity");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
// 这里用的是私有名称,由下面的start.launch文件定义
const std::string PARAM_NAME="~max_vel";
double maxVel;
bool ok = ros::param::get(PARAM_NAME, maxVel);
if (!ok) {
ROS_FATAL("can not get param ~max_vel");
return -1;
}
srand(time(0));
ros::Rate loop_rate(2);
while(ros::ok()) {
geometry_msgs::Twist msg;
msg.linear.x = maxVel*double(rand())/double(RAND_MAX);
msg.angular.z = double(rand())/double(RAND_MAX);
pub.publish(msg);
ROS_INFO("sending rand velocity cmd: linear = %f, angular = %f", msg.linear.x, msg.angular.z);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
参数"~max_vel"是私有名称,具体含义和用法见本人ROS快速入门系列博客的第三章的第2.5节的第三部分:ROS快速入门第三章 – 自定义ROS消息,编写C++ pub+sub样例
(4)编写start.launch
<launch>
<node
pkg="turtlesim"
type="turtlesim_node"
name="turtlesim"
required="true"
/>
<node
pkg="turtlesim_param"
type="turtlesim_param_set_bg"
name="turtlesim_param_set_bg"
output="screen"
/>
<node
pkg="turtlesim_param"
type="turtlesim_param_pub_max"
name="turtlesim_param_pub_max"
output="screen"
/>
// 这里指定了参数名及其value
<param name="/turtlesim_param_pub_max/max_vel" value="3" />
</launch>
(5)编写CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(turtlesim_param)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rospy
std_srvs
turtlesim
)
catkin_package(CATKIN_DEPENDS geometry_msgs roscpp rospy std_srvs turtlesim)
include_directories(${catkin_INCLUDE_DIRS})
add_executable(${PROJECT_NAME}_set_bg src/set_bg.cpp)
add_executable(${PROJECT_NAME}_pub_max src/pubvel_with_max.cpp)
target_link_libraries(${PROJECT_NAME}_set_bg
${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_pub_max
${catkin_LIBRARIES}
)
(6)编译并运行
cd ~/catkin_ws
catkin_make --source src/cpp/turtlesim_param/
source devel/setup.bash
roslaunch turtlesim_param start.launch
3 总结
本文中的例子放在了本人的github上: turtlesim_param。