ROS学习：roscpp文件编写（初级）

关于ROSCPP文件的编写，可参考：http://wiki.ros.org/roscpp?distro=melodic

一、前期知识储备

1、创建msg和srv

1.1 msg和srv介绍

msg：msg文件是描述ros消息字段的简单文本文件。它用于为不同语言的消息生成源代码。
srv：srv文件描述了一个服务。它由两部分组成：请求和响应。

msg文件存储在包的msg目录中，srv文件存储在srv目录中。
msg只是简单的文本文件，每行有一个字段类型和字段名。可以使用的字段类型有：
int8, int16, int32, int64 (plus uint*)
float32, float64
string
time, duration
other msg files
variable-length array[] and fixed-length array[C]
ros中还有一种特殊类型：header，header包含ros中常用的时间戳和坐标帧信息。您经常会看到msg文件的第一行有头文件头。

1.2、msg

1.2.1、创建msg

$ mkdir msg
$ echo "int64 num" > msg/Num.msg

上面的msg文件只包含一行。当然，您可以通过添加多个元素来创建更复杂的文件，每行一个，如下所示：

string first_name
string last_name
uint8 age
uint32 score

1.2.2、修改package.xml和cmakelists.txt

需要确保msg文件变成C++、Python和其他语言的源代码。打开package.xml，并确保这两行位于其中且未注释。在构建时，我们需要“message_generation”，而在运行时，我们只需要“message_runtime”。

  message_generation
  message_runtime

打开cmakelists.txt，添加以下内容：

find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
   message_generation
)


catkin_package(
  ...
  CATKIN_DEPENDS message_runtime ...
  ...)
 

 add_message_files(
  FILES
  Num.msg
)


generate_messages(
  DEPENDENCIES
  std_msgs
)

1.2.3、使用msg

以上是创建msg所需要做的。让我们确保ros可以使用rosmsg show命令看到它。

$ rosmsg show beginner_tutorials/Num

or

$ rosmsg show Num

1.3、srv

1.3.1、创建srv

srv文件和msg文件一样，只是它们包含两部分：请求和响应。这两部分用一条
“- - -”线隔开。

int64 A
int64 B
---
int64 Sum

1.3.2、修改package.xml和cmakelists.txt

打开package.xml，并确保这两行位于其中且未注释。

  message_generation
  message_runtime

打开cmakelists.txt。

find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
   message_generation
)


catkin_package(
  ...
  CATKIN_DEPENDS message_runtime ...
  ...)
 

add_message_files(
  FILES
   AddTwoInts.srv
)

1.3.3、使用srv

$ rossrv show beginner_tutorials/AddTwoInts

or

$ rossrv show AddTwoInts

二、编写简单的Publisher 和 Subscriber

关于Cmakelists.txt和Package.xml中的写法，可参考：
http://docs.ros.org/latest/api/catkin/html/howto/format2/building_msgs.html

1、编写publisher节点

“节点”是ros术语，指连接到ros网络的可执行文件。在这里，我们将创建一个publisher（“talker”）节点，该节点将持续广播消息。

创建src/talker.cpp文件，并在其中粘贴以下内容：

//ros/ros.h是一个方便的工具，包括使用ros系统中最常见的公共部分所需的所有头文件。
#include "ros/ros.h"
//包括std_msgs/string消息，它位于std_msgs包中。
#include "std_msgs/String.h"

#include 

//本教程演示如何通过ros系统简单地发送消息
int main(int argc, char **argv)
{
//ros::init()函数需要查看argc和argv，以便能够执行命令行中提供的任何ros
//参数和名称重新映射。 init()的第三个参数是节点的名称。
  ros::init(argc, argv, "talker");
//NodeHandle是与ros系统通信的主要接入点。构造的第一个NodeHandle将完全
//初始化该节点，而销毁的最后一个NodeHandle将关闭该节点。
  ros::NodeHandle n;
//advertise()函数的作用是告诉ros要发布一个给定的主题名。这将调用对ros主节点
//的调用，主节点保存谁正在发布谁正在订阅的注册表。
//执行advertise()函数调用后，主节点将通知尝试订阅此主题名称的任何节点，然后它们将//与此节点协商对等连接。advertise()函数返回一个publisher对象，该对象允许您通过调//用publish()发布关于该主题的消息。一但返回的发布者对象的所有副本都将被销毁，主题//将自动取消通知。
//advertide()的第二个参数是用于发布消息的消息队列的大小。如果消息发布的速度比我们
//发送它们的速度快，那么这里的数字指定在丢弃一些消息之前要缓冲多少消息。
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
//ros::Rate对象可以允许你指定自循环的频率。它会追踪记录自上一次调用Rate::sleep()
//后时间的流逝，并休眠直到一个频率周期的时间。在这个例子中，我们让它以10hz的频率运行。
  ros::Rate loop_rate(10);
//我们已经发送了多少消息的计数。这用于为每条消息创建唯一的字符串。
  int count = 0;
  while (ros::ok())
  {
//这是一个消息对象。你用数据填充它，然后发布它。
    std_msgs::String msg;

    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();
//ROS_INFO和friends是printf/cout的替换。
    ROS_INFO("%s", msg.data.c_str());
//publish()函数是发送消息的方式。参数是消息对象。此对象的类型必须与作为广播
//调用的模板参数给定的类型一致，正如在上面的构造函数中所做的那样。
    chatter_pub.publish(msg);
//ROS消息回调处理函数。对于这个简单的程序来说是不必要的，因为没有收到任何回调。
    ros::spinOnce();
//调用ros::Rate对象来休眠一段时间以使得发布频率为10hz
    loop_rate.sleep();
    ++count;
  }
  return 0;
}

2、编写Subscriber节点

#include "ros/ros.h"
#include "std_msgs/String.h"
//这是一个回调函数，当新消息到达chatter主题时将调用它。消息在共享所有权的智能指针
//中传递，这意味着您可以根据需要将其存储起来，而不必担心它会在您下面被删除，也不必
//复制底层数据。
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}
//本教程演示如何通过ros系统简单地接收消息。
int main(int argc, char **argv)
{
 //ros初始化；
  ros::init(argc, argv, "listener");
//创建句柄
  ros::NodeHandle n;
//subscribe()函数是告诉ros想要接收关于给定主题的消息。消息被传递到回调函数，
//这里为chattercallback。每当新消息到达时，ros将调用chattercallback（）函数。
//subscribe()函数的第一个参数是订阅的主题名，第二个参数是消息队列的大小。在这种情
//况下，如果队列达到1000条消息，当新消息到达时，我们将开始丢弃旧消息。
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
//spin()为ROS消息回调处理函数。spin()进入一个循环，尽可能快地调用消息回调。主程序
//到这儿就不往下执行了。
  ros::spin();
  return 0;
}

三、编写简单的Service 和 Client

1、编写Service节点

请确保已创建本教程中所需的服务，即创建addtwoints.srv。

//AddTwoints.h是创建的srv文件生成的头文件。
#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
//此函数提供两个整数相加的服务，它接受srv文件中定义的请求和响应类型并返回布尔值。
bool add(beginner_tutorials::AddTwoInts::Request  &req,
         beginner_tutorials::AddTwoInts::Response &res)
{
//两个整数相加并存储在响应中。记录有关请求和响应的一些信息。最后，服务完成时返回true。
  res.sum = req.a + req.b;
  ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
  ROS_INFO("sending back response: [%ld]", (long int)res.sum);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_server");
  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService("add_two_ints", add);
  ROS_INFO("Ready to add two ints.");
  ros::spin();

  return 0;
}

2、编写Client节点

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
#include 

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_client");
  if (argc != 3)
  {
    ROS_INFO("usage: add_two_ints_client X Y");
    return 1;
  }

  ros::NodeHandle n;
//为add_two_ints服务创建一个客户端。ros::serviceClient对象用于稍后调用服务。
  ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");
//在这里实例化一个自动生成的服务类，并将值赋给它的请求成员。服务类包含两个成员：请
//求和响应。它还包含两个类定义：请求和响应。
  beginner_tutorials::AddTwoInts srv;
  srv.request.a = atoll(argv[1]);
  srv.request.b = atoll(argv[2]);
//这实际上就是服务。由于服务调用被阻塞，一旦调用完成，它将返回。如果服务调用成
//功，call()将返回true，srv.response中的值有效。如果调用未成功，call()将返回
//false，srv.response中的值无效。
  if (client.call(srv))
  {
    ROS_INFO("Sum: %ld", (long int)srv.response.sum);
  }
  else
  {
    ROS_ERROR("Failed to call service add_two_ints");
    return 1;
  }

  return 0;
}

3、修改Cmakelist和Package

3.1、Cmakelist

add_executable(add_two_ints_server src/add_two_ints_server.cpp)
target_link_libraries(add_two_ints_server ${catkin_LIBRARIES})
add_dependencies(add_two_ints_server beginner_tutorials_gencpp)

add_executable(add_two_ints_client src/add_two_ints_client.cpp)
target_link_libraries(add_two_ints_client ${catkin_LIBRARIES})
add_dependencies(add_two_ints_client beginner_tutorials_gencpp)

3.2、Package

四、在roscpp中使用参数

1、检索参数

使用NodeHandle检索参数有两种方法。

1.1、getParam()

getParam()有许多重载形式，但它们都遵循相同的基本形式：

bool getParam (const std::string& key, parameter_type& output_value) const 

$\bullet$ key是资源名称；
$\bullet$ output_value是放置检索的数据的位置，其中parameter_type是bool、int、double、string或特殊的XmlRpcValue类型之一，它可以表示任何类型，还可以为lists/maps。
注意，getParam()返回一个bool类型，它返回检索参数是否成功。

std::string s;
n.getParam("my_param", s);

检索 my_param，将其赋值给s。

std::string s;
if (n.getParam("my_param", s))
{
  ROS_INFO("Got param: %s", s.c_str());
}
else
{
  ROS_ERROR("Failed to get param 'my_param'");
}

1.2、param()

param()类似于getparam()，但允许您在无法检索参数的情况下指定默认值。

  int i;
  n.param("my_num", i, 42);

检索参数my_num，将其赋值给i，如果没有检索成功，则i=42 。

2、设置参数

通过setparam()方法设置参数。

n.setParam("my_param", "hello there");

setparam()和getparam()一样，可以接受bool、int、double、string和特殊的XmlRpcValue类型。

3、删除参数

删除参数是通过deleteParam()方法完成的。

n.deleteParam("my_param");

4、检查存在性

通过hasParam() 检查参数是否存在。

  if (!n.hasParam("my_param"))
  {
    ROS_INFO("No param named 'my_param'");
  }

5、搜索参数

通过searchParam()搜索参数。

n.searchParam("b", param_name)

b为要搜索的文件夹，param_name为要搜索的参数名。

五、回调函数采用类(class)

1、Listener

假设Listener如下：

class Listener
{
public:
  void callback(const std_msgs::String::ConstPtr& msg);
};

主函数中使用该函数的NodeHandle::subscribe()应该为如下方式：

Listener listener;
ros::Subscriber sub = n.subscribe("chatter", 1000, &Listener::callback, &listener);

2、Service Servers

假设回调函数如下：

class AddTwo
{
public:
  bool add(roscpp_tutorials::TwoInts::Request& req,
           roscpp_tutorials::TwoInts::Response& res);
};

则主函数应为如下方式：

AddTwo a;
ros::ServiceServer ss = n.advertiseService("add_two_ints", &AddTwo::add, &a);

六、在roscpp中使用Timers

Timers允许以指定的速率进行回调。它们是ROS的一种更灵活和有用的形式，在编写简单的发布者和订阅方教程时使用的速率。

1、使用Timers

ros::Timer timer = n.createTimer(ros::Duration(0.1), timerCallback);
void timerCallback(const ros::TimerEvent& e);//回调函数

2、例子

#include "ros/ros.h"

/**
 * This tutorial demonstrates the use of timer callbacks.
 */

void callback1(const ros::TimerEvent&)
{
  ROS_INFO("Callback 1 triggered");
}

void callback2(const ros::TimerEvent&)
{
  ROS_INFO("Callback 2 triggered");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;

  /**
   * Timers allow you to get a callback at a specified rate.  Here we create
   * two timers at different rates as a demonstration.
   */
  ros::Timer timer1 = n.createTimer(ros::Duration(0.1), callback1);
  ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2);

  ros::spin();

  return 0;
}