ROS学习记录(二)

1、创建ROS消息和ROS服务

消息(msg): msg文件就是一个描述ROS中所使用消息类型的简单文本。它们会被用来生成不同语言的源代码。
服务(srv): 一个srv文件描述一项服务。它包含两个部分：请求和响应。
msg文件存放在package的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，它含有时间戳和坐标系信息。(PS:上面的数据类型中没用Header,也没有kill等等的，感觉把header理解为结构体或者类还更好点？.)

1.1、使用msg

先创建一个msg:

想直接用roscd 进入的，但是没想到找不到。。

吐了，吐了，每次重启电脑，想用roscd，就要source setup.bash一次。
永久设置source setup.bash方法：添加环境setup.bash到bashrc
gedit ~/.bashrc（打开文件）
在文件最后加上source ~/catkin_ws/devel/setup.bash并保存。
然后运行source ~/.bashrc使之生效。

重启看看，还用要source setup.bash后，roscd才找得到文件吗？

事实证明是有效果的，这个.bashrc文件是ubuntu系统运行的时候要把上面的命令都运行一遍，我们把source setup.bash加在上面，相当于ubuntu启动就把这条命令运行了一遍。
接下来接着创建msg:
mkdir msg(创建文件夹)
echo “int64 num” > msg/Num.msg（输入一条语句到Num.msg里面）
查看package.xml, 确保它包含一下两条语句，没有的话就加上去。

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>

打开CMakeLists.txt文件，
增加：
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation)
在catkin_package里面增加CATKIN_DEPENDS message_runtime
增加或者把add_message_files那段被#注释的修改成
add_message_files(
FILES
Num.msg
)
增加generate_messages()（这段别加，加了后面就是这段出错，因为后面会加上一个generate_messages()）
(PS:CMakeLists.txt文件里面都有模板这样的，照着改就行了)

使用rosmsg show beginner_tutorials/Num检查ROS是否能够识消息。

1.2使用srv

创建一个srv:
roscd beginner_tutorials(进入文件夹)
mkdir srv（创建文件夹）
roscp rospy_tutorials AddTwoInts.srv srv/AddTwoInts.srv（复制一个服务文件）
同样打开CMakeLists.txt文件
在find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation)这段添加message_generation
增加
add_service_files(
FILES
AddTwoInts.srv
)

输入rossrv show beginner_tutorials/AddTwoInts检查ROS是否能够识该服务。

继续打开CMakeLists.txt文件
添加下面这段或者把下面这段前面的#号去除
generate_messages(
DEPENDENCIES
std_msgs
)

回到catkin_ws目录下运行catkin_make

报错，CMakeLists.txt文件有问题，generate_messages重复，在文件的222行。
把generate_messages删除，运行成功。

2、编写消息发布器和订阅器

切换到beginner_tutorials路径下
mkdir -p ~/catkin_ws/src/beginner_tutorials/src(创建文件夹，然而实际上是有的因为以前创建过，虽然我也忘了啥时候创建的)
创建src/talker.cpp 文件
本来准备用rosed来创建的，但是不知道怎么弄。还是直接code吧。
安装个中文插件。

复制粘贴下面内容：

#include "ros/ros.h"
#include "std_msgs/String.h" 
#include <sstream> 
   /**
    * This tutorial demonstrates simple sending of messages over the ROS system.
    */
   //这是一个回调函数，当接收到 chatter 话题的时候就会被调用。
   //消息是以 boost shared_ptr 指针的形式传输，意味着可以存储它而又不需要复制数据。
   void chatterCallback(const std_msgs::String::ConstPtr& msg){
     ROS_INFO("I heard: [%s]", msg->data.c_str());
   }
   int main(int argc, char **argv)
   {
     /**
      * The ros::init() function needs to see argc and argv so that it can perform
      * any ROS arguments and name remapping that were provided at the command line. For programmatic
      * remappings you can use a different version of init() which takes remappings
      * directly, but for most command-line programs, passing argc and argv is the easiest
      * way to do it.  The third argument to init() is the name of the node.
      *
      * You must call one of the versions of ros::init() before using any other
      * part of the ROS system.
      */
     //初始化一个叫talker的节点
     ros::init(argc, argv, "talker");
   
     /**
      * NodeHandle is the main access point to communications with the ROS system.
      * The first NodeHandle constructed will fully initialize this node, and the last
      * NodeHandle destructed will close down the node.
      */
     //为节点创建handle,第一个初始化节点，第二个清除节点所用的资源
     ros::NodeHandle n;
   
     /**
      * The advertise() function is how you tell ROS that you want to
      * publish on a given topic name. This invokes a call to the ROS
      * master node, which keeps a registry of who is publishing and who
      * is subscribing. After this advertise() call is made, the master
      * node will notify anyone who is trying to subscribe to this topic name,
      * and they will in turn negotiate a peer-to-peer connection with this
      * node.  advertise() returns a Publisher object which allows you to
      * publish messages on that topic through a call to publish().  Once
      * all copies of the returned Publisher object are destroyed, the topic
      * will be automatically unadvertised.
      *
      * The second parameter to advertise() is the size of the message queue
      * used for publishing messages.  If messages are published more quickly
      * than we can send them, the number here specifies how many messages to
      * buffer up before throwing some away.
      */
     //发送一个信息到名为chatter的话题上，这个信息类型是std_msgs::String，推送的队列大小是1000
     ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);

     //循环发送，发送频率10HZ
     ros::Rate loop_rate(10);
   
     /**
      * A count of how many messages we have sent. This is used to create
      * a unique string for each message.
      */
     int count = 0;
     //有4种情况ros::ok()返回false,1、触发中断。2、被另一个同名节点踢出ROS网络
     //3、ros::shutdown() 被程序的另一部分调用。4、节点中的所有 ros::NodeHandles 都已经被销毁。
     while (ros::ok())
     {
       /**
        * This is a message object. You stuff it with data, and then publish it.
      */
       //声明一个msg,并把字符串赋值给msg中的data
       std_msgs::String msg;
   
       std::stringstream ss;
       ss << "hello world " << count;
       msg.data = ss.str();
       
       //打印信息，代替cout跟print
       ROS_INFO("%s", msg.data.c_str());
   
       /**
        * The publish() function is how you send messages. The parameter
        * is the message object. The type of this object must agree with the type
        * given as a template parameter to the advertise<>() call, as was done
        * in the constructor above.
       */

      //发布消息
      chatter_pub.publish(msg);

      //在这个例子中并不是一定要调用 ros::spinOnce()，因为我们不接受回调。
      //然而，如果你的程序里包含其他回调函数，最好在这里加上 ros::spinOnce()这一语句，
      //否则你的回调函数就永远也不会被调用了。
      ros::spinOnce();
  
      //延迟一段时间
      loop_rate.sleep();
      ++count;
    }

    return 0;
  }

头文件报错，是因为没搭建ROS+VSCODE的环境，那就搭建个ROS+VSCODE的环境

2.1、搭建ROS+Vscode开发环境

安装插件：
安装c/c++插件（输出C++就能看到）
安装ROS插件（输入ROS第一个就是）
接下来可选：
安装txt插件(输入txt第一个)
安装msg插件(输入msg第一个)
修改配置任务：
快捷键shift+ctrl+b,选择catkin_make:build,
修改成:

{
	"version": "2.0.0",
	"tasks": [
		{
			"type": "shell",
			"command": "catkin_make",
			"args": [],
			"group": {"kind":"build","isDefault":true},
			"label": "catkin_make: build",
			"presentation":{
				"reveal": "always"
			},
			"problemMatcher": "$msCompile"
		}
	]
}

现在，在头文件那就没报错了。

接下来是订阅器
在src文件下新建一个listener.cpp
复制粘贴以下代码

#include "ros/ros.h"
#include "std_msgs/String.h"

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
//回调函数
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line.
   * For programmatic remappings you can use a different version of init() which takes
   * remappings directly, but for most command-line programs, passing argc and argv is
   * the easiest way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  //初始化一个较listener的节点
  ros::init(argc, argv, "listener");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  //为节点创建handle
  ros::NodeHandle n;

  /**
   * The subscribe() call is how you tell ROS that you want to receive messages
   * on a given topic.  This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing.  Messages are passed to a callback function, here
   * called chatterCallback.  subscribe() returns a Subscriber object that you
   * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
   * object go out of scope, this callback will automatically be unsubscribed from
   * this topic.
   *
   * The second parameter to the subscribe() function is the size of the message
   * queue.  If messages are arriving faster than they are being processed, this
   * is the number of messages that will be buffered up before beginning to throw
   * away the oldest ones.
   */
  //订阅 chatter 话题上的消息。当有消息发布到这个话题时，
  //ROS 就会调用 chatterCallback() 函数。第二个参数是队列大小1000
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

  /**
   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
   * callbacks will be called from within this thread (the main one).  ros::spin()
   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
   */
  //ros::spin() 进入自循环，可以尽可能快的调用消息回调函数。
  //如果没有消息到达，它不会占用很多 CPU，所以不用担心。一旦 ros::ok() 返回 false，
  //ros::spin() 就会立刻跳出自循环。
  ros::spin();

  return 0;
}

打开CMakeLists.txt文件，在文件末尾加上：

include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker beginner_tutorials_generate_messages_cpp)

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener beginner_tutorials_generate_messages_cpp)

返回 cd ~/carkin_ws目录下，运行catkin_make

3、测试发布器和订阅器

在roscore运行的情况下，新建一个终端
cd ~/catkin_ws(进入文件夹)
rosrun beginner_tutorials talker（运行beginner_tutorials里面的talker节点）
一直在不断刷新信息。

再新建一个终端，rosrun beginner_tutorials listener启动订阅器。

用rqt_graph看下，就是刚才我们新建的节点talker，节点listener跟话题chatter.

4、编写简单的服务器和客户端

4.1编写服务器端节点

操作与编写发布器、订阅器差不多。
roscd beginner_tutorials(进入文件夹)
在src文件中创建add_two_ints_server.cpp文件
复制粘贴以下代码

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
/*
这个函数提供两个int值求和的服务，int值从request里面获取，
而返回数据装入response内，这些数据类型都定义在srv文件内部，
函数返回一个boolean值。
*/
bool add(beginner_tutorials::AddTwoInts::Request  &req,
    beginner_tutorials::AddTwoInts::Response &res){
        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;
    //建立发布service
    ros::ServiceServer service = n.advertiseService("add_two_ints", add);
    ROS_INFO("Ready to add two ints.");
    ros::spin();
    return 0;
}

4.2编写客户端节点

在src文件中创建add_two_ints_client.cpp文件
复制粘贴以下代码

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
#include <cstdlib>

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 service创建一个client。ros::ServiceClient 对象待会用来调用service。
    ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");
    /*
    实例化一个由ROS编译系统自动生成的service类，并给其request成员赋值。
    一个service类包含两个成员request和response。
    同时也包括两个类定义Request和Response
    */
    beginner_tutorials::AddTwoInts srv;
    srv.request.a = atoll(argv[1]);
    srv.request.b = atoll(argv[2]);
    /*
    这段代码是在调用service。由于service的调用是模态过程
    （调用的时候占用进程阻止其他代码的执行），一旦调用完成，将返回调用结果。
    如果service调用成功，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;
}

打开CMakeLists.txt文件，在文件末尾加上：

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)

进入catkin_ws目录，运行catkin_make

5、测试服务器和客户端

运行Service：
rosrun beginner_tutorials add_two_ints_server

新建一个终端，运行客户端：
rosrun beginner_tutorials add_two_ints_client 1 3

客户端发送请求，1，3，服务器端接受到后进行相加，回调给客户端。

6、录制与回放数据

6.1、录制数据

启动乌龟窗口节点：rosrun turtlesim turtlesim_node
启动键盘控制节点：rosrun turtlesim turtle_teleop_key
新建一个文件夹：mkdir ~/bagfiles
进入这个新建的文件夹： cd ~/bagfiles
开始录制：rosbag record -a(想结束录制就ctrl+c直接中断)

6.2、回放数据

先ls下看看刚才生成的数据名字。

加载数据：rosbag info 数据名字(用tab键比较简便)

回放数据：rosbag play 数据名字

可以用rosbag play -r 2 数据名字来改变消息发布速率。

6.3、录制数据子集

当运行一个复杂的系统时很多话题被发布，有些话题会发布大量数据。在这种系统中，要想把所有话题都录制保存到硬盘上的单个bag文件中是不切实际的。rosbag record命令支持只录制某些特别指定的话题到单个bag文件中，这样就允许用户只录制他们感兴趣的话题。
先看下有哪些话题rostopic list

运行rosbag record -O subset /turtle1/cmd_vel /turtle1/pose.
表示把数据保存在subset中，只保存/turtle1/cmd_vel和/turtle1/pose这两个话题的数据。

7、手动创建ROS package

在catkin_ws目录下mkdir -p src/foobar建立文件夹。
进入文件夹：cd src/foobar
添加配置文件：gedit package.xml
复制粘贴以下语句：

<package>
  <name>foobar</name>
  <version>1.2.4</version>
  <description>
  This package provides foo capability.
  </description>
  <maintainer email="foobar@foo.bar.willowgarage.com">PR-foobar</maintainer>
  <license>BSD</license>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>

  <run_depend>roscpp</run_depend>
  <run_depend>std_msgs</run_depend>
</package>

检测以下ros是否能找到这个包:rospack find foobar

建立CMakeLists.txt文件：gedit CMakeLists.txt
复制粘贴以下数据：

cmake_minimum_required(VERSION 2.8.3)
project(foobar)
find_package(catkin REQUIRED roscpp std_msgs)
catkin_package()

在catkin_ws目录下编译一下

可以看到增加了两个包，一个是刚才我们弄的，一个是之前我们做测试弄的。

8、管理系统依赖项

ROS packages有时会需要操作系统提供一些外部函数库，这些函数库就是所谓的“系统依赖项”。在一些情况下，这些依赖项并没有被系统默认安装，因此，ROS提供了一个工具rosdep来下载并安装所需系统依赖项。
进入turtlesim目录：roscd turtlesim
就能用rosdep 安装包。

9、ROS在多机器人上的使用

假如说我们希望在两台机器上分别运行talker / listener， 主机名分别为 marvin 和 hal.登陆主机名为marvin的机器。
一台机器输入：ssh hal
然后启动ROS：roscore
新建一个终端输入：
ssh hal
export ROS_MASTER_URI=http://hal:11311
rosrun rospy_tutorials listener.py
运行listener节点。
在另一台机器上输入：ssh marvin
启动ros:roscore
同样新建一个终端输入：
ssh marvin
export ROS_MASTER_URI=http://hal:11311
rosrun rospy_tutorials talker.py
就能看到机器hal上的listener正在接收来自marvin机器上talker发布的消息。(PS:可惜目前手上没两台都装了ubuntu的机器，不能测试下)。

10、在python中使用C++类

我还没运行下python的例子呢。刚好刚才新建的foobar包是空包，就用这个包来做实验。

10.1写发布器与订阅器并测试（Python）

进入目录：roscd foobar
创建文件夹：mkdir scripts
进入文件夹：cd scripts
下载代码：wget https://raw.github.com/ros/ros_tutorials/kinetic-devel/rospy_tutorials/001_talker_listener/talker.py
在CMakeLists.txt文件中增加如下信息：

catkin_install_python(PROGRAMS scripts/talker.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

继续进入scripts目录下载代码：wget https://raw.github.com/ros/ros_tutorials/kinetic-devel/rospy_tutorials/001_talker_listener/listener.py
继续打开CMakeLists.txt
增加如下信息：

catkin_install_python(PROGRAMS scripts/talker.py scripts/listener.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

进入目录catkin:cd ~/catkin_ws
运行：catkin_make

测试：rosrun foobar talker.py
报错。

缩进问题，改下就行，写的时候没注意到。

接着新建终端运行rosrun foobar listener.py

10.2python中使用c++类

建立头文件
进入放包的目录：cd ~/catkin_ws/src
创建包并依赖roscpp,rospy,std_msgs:catkin_create_pkg python_bindings_tutorial rospy roscpp std_msgs
进入include目录：cd python_bindings_tutorial/include
打开vscode:code
把code的文件夹选在当前的文件夹。
新建add_two_ints.h
输入以下语句：

#ifndef PYTHON_BINDINGS_TUTORIAL_ADD_TWO_INTS_H
#define PYTHON_BINDINGS_TUTORIAL_ADD_TWO_INTS_H
#include <std_msgs/Int64.h>
namespace python_bindings_tutorial {
class AddTwoInts{
    public:
    std_msgs::Int64 add(const std_msgs::Int64& a, const std_msgs::Int64& b);
};
}
#endif // PYTHON_BINDINGS_TUTORIAL_ADD_TWO_INTS_H   

把code的文件下选到~/catkin_ws/src/python_bindings_tutorial/src
新建文件add_two_ints.cpp
输入以下语句：

#include <python_bindings_tutorial/add_two_ints.h>
using namespace python_bindings_tutorial;
std_msgs::Int64 AddTwoInts::add(const std_msgs::Int64& a, const std_msgs::Int64& b){
    std_msgs::Int64 sum;
    sum.data = a.data + b.data;
    return sum;
}

建立C++部分，c++包装器将输入从序列化的内容转换为c++消息实例，并将c++消息实例的输出转换为序列化的内容。
继续在这个目录下建立文件add_two_ints_wrapper.cpp
输入以下语句：

#include <boost/python.hpp>
#include <string>

#include <ros/serialization.h>
#include <std_msgs/Int64.h>

#include <python_bindings_tutorial/add_two_ints.h>

/* Read a ROS message from a serialized string.
*/
template <typename M>
M from_python(const std::string str_msg)
{
size_t serial_size = str_msg.size();
boost::shared_array<uint8_t> buffer(new uint8_t[serial_size]);
for (size_t i = 0; i < serial_size; ++i)
{
  buffer[i] = str_msg[i];
}
  ros::serialization::IStream stream(buffer.get(), serial_size);
  M msg;
  ros::serialization::Serializer<M>::read(stream, msg);
  return msg;
}

/* Write a ROS message into a serialized string.
*/
template <typename M>
std::string to_python(const M& msg)
{
  size_t serial_size = ros::serialization::serializationLength(msg);
  boost::shared_array<uint8_t> buffer(new uint8_t[serial_size]);
  ros::serialization::OStream stream(buffer.get(), serial_size);
  ros::serialization::serialize(stream, msg);
  std::string str_msg;
  str_msg.reserve(serial_size);
  for (size_t i = 0; i < serial_size; ++i)
  {
    str_msg.push_back(buffer[i]);
  }
  return str_msg;
}

class AddTwoIntsWrapper : public python_bindings_tutorial::AddTwoInts
{
  public:
    AddTwoIntsWrapper() : AddTwoInts() {}

    std::string add(const std::string& str_a, const std::string& str_b)
    {
      std_msgs::Int64 a = from_python<std_msgs::Int64>(str_a);
      std_msgs::Int64 b = from_python<std_msgs::Int64>(str_b);
      std_msgs::Int64 sum = AddTwoInts::add(a, b);

      return to_python(sum);
    }
};

BOOST_PYTHON_MODULE(_add_two_ints_wrapper_cpp)
{
  boost::python::class_<AddTwoIntsWrapper>("AddTwoIntsWrapper", boost::python::init<>())
    .def("add", &AddTwoIntsWrapper::add)
    ;
}

建立python部分，Python包装器将来自Python消息实例的输入转换为序列化的内容，并将来自序列化内容的输出转换为Python消息实例。从Python串行化内容(str)到c++串行化内容(std::string)的转换构建在Boost Python库中。
进入文件夹：roscd python_bindings_tutorial/src
建立文件夹：mkdir python_bindings_tutorial(把python文件放在这个文件夹下)
进入文件夹：cd python_bindings_tutorial
新建文件_add_two_ints_wrapper_py.py，并输入以下内容：

from StringIO import StringIO

import rospy
from std_msgs.msg import Int64

from python_bindings_tutorial._add_two_ints_wrapper_cpp import AddTwoIntsWrapper


class AddTwoInts(object):
    def __init__(self):
        self._add_two_ints = AddTwoIntsWrapper()

    def _to_cpp(self, msg):
        """Return a serialized string from a ROS message

        Parameters
        ----------
        - msg: a ROS message instance.
        """
        buf = StringIO()
        msg.serialize(buf)
        return buf.getvalue()

    def _from_cpp(self, str_msg, cls):
        """Return a ROS message from a serialized string

        Parameters
        ----------
        - str_msg: str, serialized message
        - cls: ROS message class, e.g. sensor_msgs.msg.LaserScan.
        """
        msg = cls()
        return msg.deserialize(str_msg)

    def add(self, a, b):
        """Add two std_mgs/Int64 messages

        Return a std_msgs/Int64 instance.

        Parameters
        ----------
        - a: a std_msgs/Int64 instance.
        - b: a std_msgs/Int64 instance.
        """
        if not isinstance(a, Int64):
            rospy.ROSException('Argument 1 is not a std_msgs/Int64')
        if not isinstance(b, Int64):
            rospy.ROSException('Argument 2 is not a std_msgs/Int64')
        str_a = self._to_cpp(a)
        str_b = self._to_cpp(b)
        str_sum = self._add_two_ints.add(str_a, str_b)
        return self._from_cpp(str_sum, Int64)

创建文件__init__.py
输入以下语句：

from python_bindings_tutorial._add_two_ints_wrapper_py import AddTwoInts

进入CMakeLists.txt.复制粘贴以下内容：

cmake_minimum_required(VERSION 3.0.2)
project(python_bindings_tutorial)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  roscpp_serialization
  std_msgs
)

## Both Boost.python and Python libs are required.
find_package(Boost REQUIRED COMPONENTS python)
find_package(PythonLibs 2.7 REQUIRED)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
catkin_python_setup()

###################################
## catkin specific configuration ##
###################################
catkin_package(
        INCLUDE_DIRS include
        LIBRARIES add_two_ints _add_two_ints_wrapper_cpp
        CATKIN_DEPENDS roscpp
        #  DEPENDS system_lib
)

###########
## Build ##
###########

# include Boost and Python.
include_directories(
        include
        ${catkin_INCLUDE_DIRS}
        ${Boost_INCLUDE_DIRS}
        ${PYTHON_INCLUDE_DIRS}
        )

## Declare a cpp library
add_library(add_two_ints src/add_two_ints.cpp)
add_library(_add_two_ints_wrapper_cpp src/add_two_ints_wrapper.cpp)

## Specify libraries to link a library or executable target against
target_link_libraries(add_two_ints ${catkin_LIBRARIES})
target_link_libraries(_add_two_ints_wrapper_cpp add_two_ints ${catkin_LIBRARIES} ${Boost_LIBRARIES})

# Don't prepend wrapper library name with lib and add to Python libs.
set_target_properties(_add_two_ints_wrapper_cpp PROPERTIES
        PREFIX ""
        LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}
        )

进入python_bindings_tutorial目录：roscd python_bindings_tutorial
新建setup.py文件
输入以下语句：

# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD

from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup

# fetch values from package.xml
setup_args = generate_distutils_setup(
    packages=['python_bindings_tutorial'],
    package_dir={'': 'src'})

setup(**setup_args)

进入catkin_ws目录：cd ~/catkin_ws
运行catkin_make

测试下，运行python3
输入：from std_msgs.msg import Int64
输入：from python_bindings_tutorial import AddTwoInts
报错：

找到路径cd ~/catkin_ws/src/python_bindings_tutorial/src/python_bindings_tutorial下的文件_add_two_ints_wrapper_py.py
把from StringIO import StringIO改成from io import StringIO
然后回到cd ~/catkin_ws目录，运行catkin_make
再接着输入：
python3
from std_msgs.msg import Int64
from python_bindings_tutorial import AddTwoInts
a = Int64(4)
b = Int64(2)
addtwoints = AddTwoInts()
sum = addtwoints.add(a, b)
接着报错，吐了，吐了

仔细的看了下，查找了下资料，是因为python3中把StringIO写在io里面了，目前不知道怎么解决，希望有大佬能教下。