ROS Package中的Python使用规范

参考：
1. https://answers.ros.org/question/192723/cant-find-python-scripts-after-sourcing/
2. http://wiki.ros.org/PyStyleGuide

最近需要在ROS下实现几个使用TensorFlow的Publisher和Server node，出现了搜索不到module或者ros找不到我的node的情况，所以在网上搜了下怎么管理用Python写的ros package，把问题解决了。这篇博客的前半部分都来自参考[1]，后半部分会讲一下参考[1]没说清楚的部分，还有我遇到的问题。

一、ROS Package结构

假设我建立了一个叫knex_pkg的包，其中包含如下四个node：

scripts/knex_arduino_connector.py
scripts/knex_scratch_connector.py
scripts/range_filter.py
scripts/range_to_laser.py

那么这个Package的结构应该如下所示：

knex_pkg/
    CMakesLists.txt
    package.xml
    launch/ # roslaunch files go here
    msg/ # msg files go here
    nodes/ # installable python nodes go here
        knex_arduino_connector.py
        knex_scratch_connector.py
        range_filter.py
        range_to_laser.py
    src/ # this is where your python modules exported as libraries or used in your nodes go.
    urdf/ # urdf and xacro files go here -- basically your robot model stuff, if any.
    scripts/ # generally non-exported python scripts -- stuff that might be called to setup your package and code gen stuff
    srv/ # service descriptions go here
    __init__.py # only necessary if you want to use anything from the scripts directory in your package src and node files
    setup.py # more on this later

二、setup.py

另外还需要在CMakesLists.txt中加一行catkin_python_setup()来处理setup.py，这一行应该在find_package()之后，在service, message generation和 add messages calls等调用之前。

cmake_minimum_required(VERSION 2.8.3)
project(knex_ros)

find_package(catkin REQUIRED COMPONENTS
  rospy
  std_msgs
  tf
  roscpp
  robot_state_publisher
  differential_drive
)
catkin_python_setup()
...

下面来讲一下setup.py，复制参考[1]里的内容：

setup.py is the python distutils setup script. It’s where you describe the structure of a python package. The python style guide for ros is pretty specific in how python packages should be layed out. Any modules used should go under src. If you are like me, and like a clearly defined source tree structure with directories as package names, your setup.py will look something like this:

## ! 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=['my_package'],
    package_dir={'': 'src'},
    requires=['std_msgs', 'rospy', 'message_filters', 'gps_common', 'sensor_msgs']
)

setup(**setup_args)

In setup_args you are defining your package by calling generate_distutils_setup.

The packages argument is an array of package names. Use dot-delimited format (my_package.subpackage) if you wish to explicitly name more than one package. I generally just use one package root and let distutils figure it out from there, and I think this is the simplest practice.

The package_dir arg allows you to specify a source directory for a package name. If you have only one package, then you can omit the package name and give a relative path to the directory containing the modules for that package. If you follow the style guide, that directory will be src. If you have multiple packages in different source directories, you can specify them in this dictionary.

The requires arg is an array of python package names that are required by your package(s). In my example setup.py above, I use std_msgs so I've included it in the list. You will probably always have rospy. I don't think this is explicitly required for ros packages, but I think it is good practice to list them here.

在src下面，如果只有一个submodule的话，可以直接命名为my_package.py，另外记得添加一个__init__.py。如果src下面有多个submodule的话，可以按如下方式安排：

my_package/ # the root of the ros package
    setup.py
    src/
        my_package/ # here's your python module root
            __init__.py # put your python code for my_package here.
            my_submodule_a/ # I prefer directories for submodules, but some prefer just a python file named my_submodule_a.py instead
                __init__.py # this is your my_package.my_submodule_a source file.
            my_submodule_b/
                __init__.py # this is your my_package.my_submodule_b source file.

就是在src下面先建立一个和package同名的文件夹，然后在这个文件夹下放submodule的文件夹，注意每一级都要有__init__.py。
这样catkin_python_setup可以让我们在可执行的python node（在rosrun之前要记得用chmod +x nodes/my_package_node.py）里面使用my_package这个命名空间来直接import刚才建立在src下面的modules，同理msg和srv。

#!/usr/bin/env python
import rospy
import roslib
roslib.load_manifest('my_package')

from std_msgs.msg import Header
from my_package.msg import MyMsg
import my_package.my_submodule_a

def run_my_node():
    init_node("my_package_node")
    spin()

if __name__ == "__main__":
    run_my_node()

三、使用实例

下图是我这两天写的一个用来从RGB图片中检测抓取姿态的包的结构，里面包含了两个算法，一个是直接从一张图中做回归得到抓取框的，速度很快，但是对使用场景限制比较大，第二个是从图片得到很多小patch，然后对每个patch做分类，效果很好，但是速度很慢，因此前一个我给写成了一个实时subscribe图片，然后publish抓取框的node，后一个写成了一个service，client发送一张RGB图片，server从图片中检测抓取姿态然后返回结果。

/node文件夹下面存放两个可执行的node文件，他们需要调用的module存放在src下面，这里两个算法我分别放在了两个文件夹下，注意每一级的__init__.py，/scripts下面是一些原本用来训练，验证以及可视化神经网络模型的脚本，他们在node运行的时候是不需要的。

如下是我在patch_based_grasp_detection_node.py中使用src文件夹下的submodule以及package中自己定义的service的方式：

#!/usr/bin/env python
import roslib
import rospkg
roslib.load_manifest('grasp_detection')

import cv2
import numpy as np
import time
from cv_bridge import CvBridge

from grasp_detection.patch_based_grasp_detection.grasp_learner import grasp_obj
from grasp_detection.patch_based_grasp_detection.grasp_predictor import Predictors
from grasp_detection.srv import GraspDetection, GraspDetectionResponse

需要注意的一个问题是，如果需要在python脚本中读取package里的文件，需要使用rospack来定位package路径才行。比如我这里需要找到存放神经网络参数的文件，我把它们都放在了/models文件夹下，那么就应该使用：

import rospkg
...
model_path = rospack.get_path('grasp_detection')+'/models/patch_based_grasp_detection/Grasp_model'