< >
Home » ROS入门教程 » ROS入门教程-1.2.6 在python中使用C++类 CATKIN方式

ROS入门教程-1.2.6 在python中使用C++类 CATKIN方式

ROS入门教程-在python中使用C++类 CATKIN方式

说明：

  • 本教程阐述一种在python中使用C++类的方法。

目录

  • 没有Nodehandle的类

    • 创建包和编写C++类
    • 绑定，C++部分
    • 绑定，Python部分
    • 结合
    • 测试

  • 有NodeHandle的类

  • 有ROS消息容器的类

    • std::vector<M>作为返回类型
    • std::vector<M>作为参数类型

介绍：

  • 本教程演示在Python使用带有ROS消息的C++类

  • 用到Boost Python库

  • 难点就是转换纯Python写的ROS消息的Python对象为等效的C++实例。

  • 这个转换通过serialization/deserialization来实现

  • 源码参考这里

  • 另一个解决方案是使用ROS服务，服务器端用C++编写封装成C++类，客户端可以用C++或Python编写，来调用服务。

  • 这个方案没有创建ROS节点，提供的类不需要用到ros::NodeHandle。

  • 另一个解决方案是为所有需要的ROS消息和依赖编写封装。一些显然过时的包装提出了这个任务的自动化解决方案：genpybindings和boost_ python_ros。

没有NodeHandle的类

  • 因为调用rospy.init_node时候是不会初始化roscpp .
  • ros::NodeHandle实例不能用在不产生错误的C++类
  • 如果C++不使用ros::NodeHandle，是没有问题的

创建包和写c++类

  • 创建包和C++类，这个类用于Python绑定，使用ROS消息作为参数和返回类型。

  • 编写类头件定义到add_two_ints.h

    catkin_create_pkg python_bindings_tutorial rospy roscpp std_msgs
    cd python_bindings_tutorial/include/python_bindings_tutorial
    touch add_two_ints.h
    rosed python_bindings_tutorial add_two_ints.h

  • include/python_bindings_tutorial/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);
};

} // namespace python_bindings_tutorial

#endif // PYTHON_BINDINGS_TUTORIAL_ADD_TWO_INTS_H
  • 编写类实现到add_two_ints.cpp
roscd python_bindings_tutorial/src
touch add_two_ints.cpp
rosed python_bindings_tutorial add_two_ints.cpp
  • 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++编写，另一个Python编写。
  • C++封装转换输入从序列化内容到C++消息实例，转换输出从C++消息实例到序列化内容
  • 实现代码文件add_two_ints_wrapper.cpp：
roscd python_bindings_tutorial/src
touch add_two_ints_wrapper.cpp
rosed python_bindings_tutorial add_two_ints_wrapper.cpp
  • src/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)
    ;
}
  • The line Error: No code_block found creates a Python module in the form of a dynamic library. The name of the module will have to be the name of the exported library in CMakeLists.txt.

绑定, Python部分

  • Python封装转换输入从Python消息实例到序列化内容，转换输出从序列化内容到Python消息实例。
  • 用Boost的Python库转换从Python序列化内容(str)到C++序列化内容(std::string)
roscd python_bindings_tutorial/src
mkdir python_bindings_tutorial
roscd python_bindings_tutorial/src/python_bindings_tutorial
touch _add_two_ints_wrapper_py.py
rosed python_bindings_tutorial _add_two_ints_wrapper_py.py
  • 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导入symbols
  • 首先创建文件：
roscd python_bindings_tutorial/src/python_bindings_tutorial
touch __init__.py
rosed python_bindings_tutorial __init__.py
  • src/python_bindings_tutorial/init.py内容如下：
from python_bindings_tutorial._add_two_ints_wrapper_py import AddTwoInts

结合

  • 编辑CMakeLists.txt
rosed python_bindings_tutorial 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文件
  • 创建setup.py文件
roscd python_bindings_tutorial
touch setup.py
  • 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_make构建包
cd ~/catkin_ws
catkin_make

测试

  • 在Python脚本或Python Shell下使用绑定
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

带NodeHandle的类

  • 正如文章开头描述的，Python调用rospy.init_node，不会初始化roscpp
  • 如果roscpp不初始化，ros::NodeHandle实例化会导致致命错误。
  • 解决方案就是提供moveit_ros_planning_interface.
  • 在Python脚本使用封装类，在实例化AddTwoInts之前需要增加两行
  • 如下：
from moveit_ros_planning_interface._moveit_roscpp_initializer import roscpp_init
roscpp_init('node_name', [])
  • 这个会创建ROS节点，相比传统的ROS的server/client实现的优点是不需要ros::NodeHandle

包含ROS消息容器的类

  • 如果类使用ROS消息容器，则必须添加额外的转换步骤。这一步对ROS不是特定的但是Boost Python库的一部分。

std::vector<M> 最为返回类型

  • 在这个文件C++封装类被定义，增加下面几行：
// Extracted from https://gist.github.com/avli/b0bf77449b090b768663.
template<class T>
struct vector_to_python
{
  static PyObject* convert(const std::vector<T>& vec)
  {
    boost::python::list* l = new boost::python::list();
    for(std::size_t i = 0; i < vec.size(); i++)
      (*l).append(vec[i]);

    return l->ptr();
  }
};

class Wrapper : public WrappedClass
{
/*
...
*/
    std::vector<std::string> wrapper_fun(const std::string str_msg)
    {
      /* ... */
    }

};

BOOST_PYTHON_MODULE(module_wrapper_cpp)
{
  boost::python::class_<Wrapper>("Wrapper", bp::init</* ... */>())
    .def("fun", &Wrapper::wrapper_fun);

  boost::python::to_python_converter<std::vector<std::string, std::allocator<std::string> >, vector_to_python<std::string> >();
}

std::vector<M>作为参数类型

Cf. Boost.Python documentation.

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

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


标签: ros入门教程