ROS与C++入门教程-动态配置-编写高级的发布器和订阅器

ROS与C++入门教程-动态配置-编写高级的发布器和订阅器

说明:

介绍编写高级的发布器和订阅器，结合参数，动态配置和自定义消息。
之前文章介绍rosmake的编译，本文介绍catkin_make的编译方法

参考：

https://github.com/tdenewiler/node_example
http://tdenewiler.github.io/node_example/ (介绍rosinstall方法)

编写高级的发布器和订阅器

本教程将向您展示如何结合几个初学者级教程来创建发布者和订阅者节点，这些节点比以前创建的节点更为完整。
此页将描述如何创建发布节点：
- 针对launch文件或命令行，初始化几个变量参数来使用参数服务器
- 通过使用动态配置服务器，在运行时修改参数来调整
- 使用自定义消息来发布话题
创建一个订阅节点与发布节点一起工作：
- 设置订阅节点以侦听指定主题上的自定义消息并打印出消息数据
使用的变量，以及发布者和订阅服务器使用的回调函数，将是本教程中描述的为此目的创建的类的一部分。

编译例子

创建两个程序类似之前的简单发布器和订阅器，同样叫talker和listener
进入工作空间，下载代码：

$ cd ~/catkin_ws/src
$ git clone https://github.com/tdenewiler/node_example.git

编译代码：

$ cd ~/catkin_ws
$ catkin_make

测试例子

新终端，执行roscore

$ roscore

新终端，执行node_example.launch

$ roslaunch node_example node_example.launch

打开两个窗口rqt_reconfigure 和 rqt_console.
rqt_reconfigure效果：
rqt_console效果：
改变rqt_reconfigure窗口的数字或字符串，在rqt_console就会有即时的改变

单独测试C++例子

新终端，执行roscore

$ roscore

新终端，执行node_example.launch

$ roslaunch node_example c++_node_example.launch

单独测试Python例子

新终端，执行roscore

$ roscore

新终端，执行node_example.launch

$ roslaunch node_example python_node_example.launch

创建自定义消息

具体创建方法，参考文章
查看消息文件vim msg/NodeExampleData.msg，内容如下：

string message
int32 a
int32 b

查看vim CMakelists.txt，相关配置内容如下：

add_message_files(
  FILES
  NodeExampleData.msg
)

动态参数配置文件

动态参数配置，参考文章
查看消息文件vim cfg/nodeExample.cfg，内容如下：

#! /usr/bin/env python

PACKAGE='node_example'
import roslib
roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()
#       Name       Type      Level Description     Default Min   Max
gen.add("message", str_t,    0,    "The message.", "hello")
gen.add("a",       int_t,    0,    "First number.", 1,     -100, 100)
gen.add("b",       int_t,    0,    "First number.", 2,     -100, 100)

exit(gen.generate(PACKAGE, "node_example", "nodeExample"))

这意味着可以更改message,a,b变量，也可以在运行中修改这些变量。
确保nodeExample.cfg有执行权限:

chmod 755 cfg/nodeExample.cfg

查看vim CMakelists.txt，相关配置内容如下：

generate_dynamic_reconfigure_options(
  cfg/nodeExample.cfg
)

Talker节点

查看代码源文件：vim node_example/src/talker.cpp ，内容如下：

#include "node_example/talker.h"

namespace node_example
{
ExampleTalker::ExampleTalker(ros::NodeHandle nh) : message_("hello"), a_(1), b_(2)
{
  // Set up a dynamic reconfigure server.
  // Do this before parameter server, else some of the parameter server values can be overwritten.
  dynamic_reconfigure::Server<node_example::nodeExampleConfig>::CallbackType cb;
  cb = boost::bind(&ExampleTalker::configCallback, this, _1, _2);
  dr_srv_.setCallback(cb);

  // Declare variables that can be modified by launch file or command line.
  int rate;

  // Initialize node parameters from launch file or command line. Use a private node handle so that multiple instances
  // of the node can be run simultaneously while using different parameters.
  ros::NodeHandle pnh("~");
  pnh.param("a", a_, a_);
  pnh.param("b", b_, b_);
  pnh.param("message", message_, message_);
  pnh.param("rate", rate, 1);

  // Create a publisher and name the topic.
  pub_ = nh.advertise<node_example::NodeExampleData>("example", 10);

  // Create timer.
  timer_ = nh.createTimer(ros::Duration(1 / rate), &ExampleTalker::timerCallback, this);
}

void ExampleTalker::timerCallback(const ros::TimerEvent &event)
{
  node_example::NodeExampleData msg;
  msg.message = message_;
  msg.a = a_;
  msg.b = b_;

  pub_.publish(msg);
}

void ExampleTalker::configCallback(node_example::nodeExampleConfig &config, uint32_t level)
{
  // Set class variables to new values. They should match what is input at the dynamic reconfigure GUI.
  message_ = config.message.c_str();
  a_ = config.a;
  b_ = config.b;
}
}

查看代码头文件：vim node_example/include/talker.h ，内容如下：

#ifndef NODE_EXAMPLE_TALKER_H
#define NODE_EXAMPLE_TALKER_H

// ROS includes.
#include "ros/ros.h"
#include "ros/time.h"

// Custom message includes. Auto-generated from msg/ directory.
#include "node_example/NodeExampleData.h"

// Dynamic reconfigure includes.
#include <dynamic_reconfigure/server.h>
// Auto-generated from cfg/ directory.
#include <node_example/nodeExampleConfig.h>

namespace node_example
{
class ExampleTalker
{
 public:
  //! Constructor.
  explicit ExampleTalker(ros::NodeHandle nh);

  //! Callback function for dynamic reconfigure server.
  void configCallback(node_example::nodeExampleConfig &config, uint32_t level);

  //! Timer callback for publishing message.
  void timerCallback(const ros::TimerEvent &event);

 private:
  //! The timer variable used to go to callback function at specified rate.
  ros::Timer timer_;

  //! Message publisher.
  ros::Publisher pub_;

  //! Dynamic reconfigure server.
  dynamic_reconfigure::Server<node_example::nodeExampleConfig> dr_srv_;

  //! The actual message.
  std::string message_;

  //! The first integer to use in addition.
  int a_;

  //! The second integer to use in addition.
  int b_;
};
}

#endif // NODE_EXAMPLE_TALKER_H

Listener节点

查看代码源文件：vim node_example/src/listener.cpp ，内容如下：

#include "node_example/listener.h"

namespace node_example
{
ExampleListener::ExampleListener(ros::NodeHandle nh)
{
  // Create a subscriber.
  // Name the topic, message queue, callback function with class name, and object containing callback function.
  sub_ = nh.subscribe("example", 10, &ExampleListener::messageCallback, this);
}

void ExampleListener::messageCallback(const node_example::NodeExampleData::ConstPtr &msg)
{
  // Note that these are only set to INFO so they will print to a terminal for example purposes.
  // Typically, they should be DEBUG.
  ROS_INFO("message is %s, a + b = %d", msg->message.c_str(), msg->a + msg->b);
}
}

查看代码头文件：vim node_example/include/listener.h ，内容如下：

#ifndef NODE_EXAMPLE_LISTENER_H
#define NODE_EXAMPLE_LISTENER_H

// ROS includes.
#include "ros/ros.h"
#include "ros/time.h"

// Custom message includes. Auto-generated from msg/ directory.
#include "node_example/NodeExampleData.h"

namespace node_example
{
class ExampleListener
{
 public:
  //! Constructor.
  explicit ExampleListener(ros::NodeHandle nh);

  //! Callback function for subscriber.
  void messageCallback(const node_example::NodeExampleData::ConstPtr &msg);

 private:
  //! Subscriber to custom message.
  ros::Subscriber sub_;
};
}

#endif // NODE_EXAMPLE_LISTENER_H

初始参数设置

有两种方法设置初始参数：
- 一个通过launch文件来设置
- 一个通过命令行来设置
在 node_example/launch/c++_node_example.launch文件
talker节点部分有四个变量message，a，b，rate可以设置：

<param name="a" value="1"/>
<param name="b" value="2"/>
<param name="message" value="hello"/>
<param name="rate" value="1"/>

也可以通过命令行设置：

rosrun node_example talker _message:="Hello world!" _a:=57 _b:=-15 _rate:=1

注意：当从命令行修改私有节点句柄参数时，~下划线已被_下划线替换。
运行listener查看效果：rosrun node_example listener
注意：我们的节点使用参数服务器的私有节点句柄。
这很重要，因为它有助于防止节点重新映射到具有独特的节点名称时候出现冲突。
例如：你可能希望单个节点启动两个实例，对于单独的节点句柄，可以具有相同的参数名称的不同值。
例如：当你有同一类型的多台相机，但要运行在不同的帧速率就很方便。

参数动态配置

动态重新配置的工具是令人敬佩，因为它们允许用户不仅在启动时修改变量，还能在运行过程中修改变量。我们已经创建了一个文件中指定的变量可动态重新配置服务器。
注意：文件node_example/package.xml，需要增加dynamic_reconfigure的依赖：

<depend>dynamic_reconfigure</depend>

在文件node_example/CMakeLists.txt，增加依赖包

find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp rospy std_msgs)

generate_dynamic_reconfigure_options(
  cfg/nodeExample.cfg
)

catkin_package(
  CATKIN_DEPENDS dynamic_reconfigure message_runtime roscpp rospy std_msgs
)

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号