ROS进阶学习（六） - 在Python里使用C++类

1 不要NodeHandle的类

因为当roscpp不是在rospy.init_node调用的时候初始化。ros::NodeHandle 类不能内有错误的用在一个C++类里。如果C++代码没有使用它，其他的地方就没有问题了。

1.1 创建一个package并且写一个C++类

这个类使用ROS message作为参数和返回类型。

cd catkin_ws_me           #(打开自己的工作区)
source ./devel/setup.bash
cd src
catkin_create_pkg python_bindings_tutorial rospy roscpp std_msgs
cd python_bindings_tutorial/include/python_bindings_tutorial
touch add_two_ints.h
vim add_two_ints.h

加入以下代码：

#ifndef PYTHON_BINDINGS_TUTORIAL_ADD_TWO_INTS_H
#define PYTHON_BINDINGS_TUTORIAL_ADD_TWO_INTS_H

#include 

namespace python_bindings_tutorial {

class AddTwoInts
{
  public:
    std_msgs::Int64 add(const std_msgs::Int64& a, const std_msgs::Int64& b);
};

} // namespace python_bindings_tutorial

#endif // PYTHON_BINDINGS_TUTORIAL_ADD_TWO_INTS_H

编写类的实现：
在python_bindings_tutorial 包下的src目录新建文件add_two_ints.cpp ，写入以下代码：

#include 

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;
}

1.2 绑定C++部分

绑定是通过两个包装类，一个用C++，一个是用Python。C++包装类将输入从序列化内容转换为C++ message，输出从C++ message转变为序列化的内容。
在刚才的目录下再新建一个文件add_two_ints_wrapper.cpp ，输入以下内容：

#include 

#include 

#include 
#include 

#include 

/* Read a ROS message from a serialized string.
  */
template 
M from_python(const std::string str_msg)
{
  size_t serial_size = str_msg.size();
  boost::shared_array 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::read(stream, msg);
  return msg;
}

/* Write a ROS message into a serialized string.
*/
template 
std::string to_python(const M& msg)
{
  size_t serial_size = ros::serialization::serializationLength(msg);
  boost::shared_array 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(str_a);
      std_msgs::Int64 b = from_python(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", boost::python::init<>())
    .def("add", &AddTwoIntsWrapper::add)
    ;
}

1.3 绑定python部分

前面的跟C++相似。将Python序列转变成C++序列需要在高级的Python库里实现。
在src文件夹里新建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)

为了能以python_bindings_tutorial.AddTwoInts这样的方式引入类，我们在init.py引入符号：
首先在刚才的目录里创建文件init.py，然后输入代码：

from python_bindings_tutorial._add_two_ints_wrapper_py import AddTwoInts

1.4 把所有东西黏在一起

编辑 CMakeLists.txt 如下：

cmake_minimum_required(VERSION 2.8.3)
project(python_bindings_tutorial)

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}
        )

C++包装器库应该跟Python模型有相同的名字，如果目标文件需要因为某些原因要指定特殊的名字，可以用set_target_properties(_add_two_ints_wrapper_cpp PROPERTIES OUTPUT_NAME correct_library_name)。
这一行：
catkin_python_setup()
这个用来输出Python模型，并且和setup.py关联。
在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)

现在可以到工作空间编译一下了。

1.5

然后可以新建一个Python文件test.py
输入：

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)
sum

这里实在看不懂，留作以后继续学习吧，也没有做出来现象。
总是提示错误：

Traceback (most recent call last):
  File "test.py", line 2, in 
    from python_bindings_tutorial import AddTwoInts
ImportError: No module named python_bindings_tutorial

算是又烂尾了一篇。