ROS中Python的使用:rospy


风格:

  • 所有Python代码必需放在一个模块的命名空间
  • ROS导出源码到依赖所在的路径,因此不能跟其他人的导入冲突
  • 推荐使用模块名作为包名
# 不带msg/srvs的情况下:

packagename
 |- src/
    |- packagename.py
 |- scripts/
    |- non-exported python files



# 带msg/srvs的情况下:
packagename
 |- src/
    |- packagename/
      |- __init__.py
      |- yourfiles.py
 |- scripts/
    |- non-exported python files

注意:py脚本要加可执行权限
chmod +x test.py

API参考:http://docs.ros.org/api/rospy/html/

1. 发布者和订阅者

发布者:

#!/usr/bin/env python
import rospy
from std_msgs.msg import String

def talker():
	# 定义发布者名称,消息类型以及队列个数
    pub = rospy.Publisher('chatter', String, queue_size=10)

	# anonymous=True 要求每个节点都有唯一的名称,避免冲突。这样可以运行多个talker.py
    rospy.init_node('talker', anonymous=True)

	# 设置发布的频率,单位是每秒次数
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        hello_str = "hello world %s" % rospy.get_time()
        rospy.loginfo(hello_str) # 在屏幕输出日志信息,写入到rosout节点
        pub.publish(hello_str) # 发布信息到主题
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
		pass

订阅者:

#!/usr/bin/env python

import rospy
from std_msgs.msg import String

def callback(data):
    rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.data)

def listener():

    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber('chatter', String, callback)

    rospy.spin()

if __name__ == '__main__':
    listener()

Launch文件:

<launch>
    <node name="talker" pkg="beginner_tutorials" type="talker.py" />
    <node name="listener" pkg="beginner_tutorials" type="listener.py" />
</launch>


$ rosmsg show std_msgs/ColorRGBA
float32 r
float32 g
float32 b
float32 a
#!/usr/bin/env python

import rospy
from std_msgs.msg import ColorRGBA
def talker():
    #pub = rospy.Publisher('chatter', String)
    pub = rospy.Publisher('chatter_color', ColorRGBA)
    rospy.init_node('talker_color')
    while not rospy.is_shutdown():
        pub.publish(a=1.0)
        rospy.sleep(1.0)
if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException: pass
#!/usr/bin/env python

import rospy
from std_msgs.msg import ColorRGBA
def callback(data):
    rospy.loginfo(rospy.get_name()+ "I heard r=%s g=%s b=%s a=%s", data.r, data.g, data.b, data.a)

def listener():
    rospy.init_node('listener_color', anonymous=True)
    rospy.Subscriber("chatter_color", ColorRGBA, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

2. CompressedImage类型的订阅者和发布者

发布者:

#!/usr/bin/env python

import sys, time
import numpy as np
from scipy.ndimage import filters
import cv2
import roslib
import rospy
from sensor_msgs.msg import CompressedImage

VERBOSE=False

class image_feature:

    def __init__(self):
        '''Initialize ros publisher, ros subscriber'''
        # topic where we publish
        self.image_pub = rospy.Publisher("/output/image_raw/compressed",
            CompressedImage,  queue_size = 10)
        # self.bridge = CvBridge()

        # subscribed Topic
        self.subscriber = rospy.Subscriber("/camera/image/compressed",
            CompressedImage, self.callback)
        if VERBOSE :
            print "subscribed to /camera/image/compressed"


    def callback(self, ros_data):
        '''Callback function of subscribed topic. 
        Here images get converted and features detected'''
        if VERBOSE :
            print 'received image of type: "%s"' % ros_data.format

        #### direct conversion to CV2 ####
        np_arr = np.fromstring(ros_data.data, np.uint8)
        image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
        
        #### Feature detectors using CV2 #### 
        # "","Grid","Pyramid" + 
        # "FAST","GFTT","HARRIS","MSER","ORB","SIFT","STAR","SURF"
        method = "GridFAST"
        feat_det = cv2.FeatureDetector_create(method)
        time1 = time.time()

        # convert np image to grayscale
        featPoints = feat_det.detect(
            cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY))
        time2 = time.time()
        if VERBOSE :
            print '%s detector found: %s points in: %s sec.'%(method,
                len(featPoints),time2-time1)

        for featpoint in featPoints:
            x,y = featpoint.pt
            cv2.circle(image_np,(int(x),int(y)), 3, (0,0,255), -1)
        
        cv2.imshow('cv_img', image_np)
        cv2.waitKey(2)

        #### Create CompressedIamge ####
        msg = CompressedImage()
        msg.header.stamp = rospy.Time.now()
        msg.format = "jpeg"
        msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
        # Publish new image
        self.image_pub.publish(msg)
        
        #self.subscriber.unregister()

def main(args):
    '''Initializes and cleanup ros node'''
    ic = image_feature()
    rospy.init_node('image_feature', anonymous=True)
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down ROS Image feature detector module"
    cv2.destroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)

3. 服务端和客户端

服务是通过srv文件定义,它包含请求信息和响应信息.rospy将这些SRV文件转换成Python源代码并创建三个类:服务的定义,请求消息和响应消息。

非常重要!!!

my_package/srv/Foo.srv → my_package.srv.Foo
my_package/srv/Foo.srv → my_package.srv.FooRequest
my_package/srv/Foo.srv → my_package.srv.FooResponse

srv/AddTwoInts.srv内容如下:

int64 A
int64 B
---
int64 Sum

服务端:

#!/usr/bin/env python

NAME = 'add_two_ints_server'

# import the AddTwoInts service
from packageName.srv import *
import rospy 

def add_two_ints(req):
    print("Returning [%s + %s = %s]" % (req.a, req.b, (req.a + req.b)))
    sum = req.a + req.b
    return AddTwoIntsResponse(sum) 			# AddTwoIntsResponse由服务生成的返回函数

def add_two_ints_server():
    rospy.init_node(NAME)

	# 处理函数调用实例化的AddTwoIntsRequest接收请求和返回实例化的AddTwoIntsResponse
    s = rospy.Service('add_two_ints', AddTwoInts, add_two_ints)
    print "Ready to add Two Ints"
    rospy.spin()

if __name__ == "__main__":
    add_two_ints_server()

客户端:

API传送门

#!/usr/bin/env python

import sys
import os
import rospy
# imports the AddTwoInts service 
from rospy_tutorials.srv import *

def add_two_ints_client(x, y):
    # 等待接入服务节点
    rospy.wait_for_service('add_two_ints')
    
    try:
        # create a handle to the add_two_ints service
        add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)   # 创建服务的处理句柄
        
        print "Requesting %s+%s"%(x, y)
        
        # 简单风格
        resp1 = add_two_ints(x, y)

        # 正式风格
        resp2 = add_two_ints.call(AddTwoIntsRequest(x, y))

        if not resp1.sum == (x + y):
            raise Exception("test failure, returned sum was %s"%resp1.sum)
        if not resp2.sum == (x + y):
            raise Exception("test failure, returned sum was %s"%resp2.sum)
        return resp1.sum
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

def usage():
    return "%s [x y]"%sys.argv[0]

if __name__ == "__main__":
    
    argv = rospy.myargv()
    if len(argv) == 1:
        import random
        x = random.randint(-50000, 50000)
        y = random.randint(-50000, 50000)
    elif len(argv) == 3:
        try:
            x = int(argv[1])
            y = int(argv[2])
        except:
            print usage()
            sys.exit(1)
    else:
        print usage()
        sys.exit(1)
    print "%s + %s = %s"%(x, y, add_two_ints_client(x, y))
<launch>
    <node name="server" pkg="beginner_tutorials" type="add_two_ints_server.py" />
</launch>

4. action服务端和客户端

Averaging.action:

#goal definition
int32 samples
---
#result definition
float32 mean
float32 std_dev
---
#feedback
int32 sample
float32 data
float32 mean
float32 std_dev

Fibonacci.action:

#goal definition
int32 order
---
#result definition
int32[] sequence
---
#feedback
int32[] sequence

服务端:

#! /usr/bin/env python

import roslib; roslib.load_manifest('packageName')
import rospy

import actionlib

import actionlib_tutorials.msg  # 导入生成的消息,action会生成用于发送目标,接受反馈的消息

class FibonacciAction(object):
  # 创建用于发布的消息 feedback/result
  _feedback = actionlib_tutorials.msg.FibonacciFeedback()
  _result   = actionlib_tutorials.msg.FibonacciResult()

  def __init__(self, name):
    self._action_name = name
    # SimpleActionServer创建服务端,需4个参数:action_name、action type、callback函数(可选)、auto_start
    self._as = actionlib.SimpleActionServer(self._action_name, actionlib_tutorials.msg.FibonacciAction, execute_cb=self.execute_cb, auto_start = False)
    self._as.start() # 启动服务器
    
  def execute_cb(self, goal):
  	# 定义回调函数,当新目标到达,rospy.loginfo 输出相关信息,可知道正在执行的action.
    r = rospy.Rate(1)
    
    # 辅助变量
    success = True
    
    # 反馈
    self._feedback.sequence = []
    self._feedback.sequence.append(0)
    self._feedback.sequence.append(1)
    
    # 为用户发布信息到控制台
    rospy.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self._action_name, goal.order, self._feedback.sequence[0], self._feedback.sequence[1]))
    
    ######################################################################################################
    # 开始执行操作
    for i in xrange(1, goal.order):
      # 检查是否客户端没有请求抢占
      if self._as.is_preempt_requested():
        rospy.loginfo('%s: Preempted' % self._action_name)
        self._as.set_preempted() # 当客户端替换了目标,服务器端应该取消目标,执行清除,调用set_preempted。preempt(抢占)
        success = False
        break
      self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
      
      # 发布反馈
      self._as.publish_feedback(self._feedback)
      # 这个步骤是不必要的,为了演示的目的,顺序被计算为1hz
      r.sleep()
    ######################################################################################################  
    
    if success:
      self._result.sequence = self._feedback.sequence
      rospy.loginfo('%s: Succeeded' % self._action_name)
      self._as.set_succeeded(self._result) # succeeded(成功)
      
if __name__ == '__main__':
  rospy.init_node('fibonacci')
  FibonacciAction(rospy.get_name()) # rospy.get_name()获取节点名称,实例化动作服务器的类
  rospy.spin() 

客户端:

#! /usr/bin/env python

import roslib; roslib.load_manifest('actionlib_tutorials') # 查找manifest文件,增加依赖到python路径,不能多次调用load_manifest
import rospy
import actionlib
import actionlib_tutorials.msg

def fibonacci_client():
    # 创建SimpleActionClient,传递操作的类型
    client = actionlib.SimpleActionClient('fibonacci', actionlib_tutorials.msg.FibonacciAction)

    # 等待连接上服务器
    client.wait_for_server()

    # 创建目标并发送到action服务器端
    goal = actionlib_tutorials.msg.FibonacciGoal(order=20)
    client.send_goal(goal)

    # 等待服务器完成操作
    client.wait_for_result()

    # 打印执行操作的结果
    return client.get_result()  # A FibonacciResult

if __name__ == '__main__':
    try:
        # 初始化rospy节点,以便SimpleActionClient可以通过ROS发布和订阅。
        rospy.init_node('fibonacci_client_py')
        result = fibonacci_client()
        print "Result:", ', '.join([str(n) for n in result.sequence])
    except rospy.ROSInterruptException:
        print "program interrupted before completion"

5. TF树的使用

广播变换

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

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
	# 处理函数针对turtle的位置信息广播平移和旋转
	# 作为变换,从"world" 坐标系到"turtleX"坐标系进行发布
    br = tf.TransformBroadcaster() # 创建广播者
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle') # 节点获取turtle参数,这个指定一个turtle名,如"turtle1" or "turtle2".
   
    # 节点订阅"turtleX/pose" 主题,指定handle_turtle_pose回调函数
    # 回调函数有参数,节点名turtlename和相关信息turtlesim.msg.Pose
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

监听变换

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('tf_turtle')

    listener = tf.TransformListener() # 创建了一个tf.transformlistener对象.一旦侦听器被创建,它就开始接收转换,并缓冲他们长达10秒。

	# 调用服务,生成小乌龟
    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')

    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
        	# 通过listener.lookupTransform函数来实现获取变换,从/turtle2变换到/turtle1,rospy.Time(0)获取最新变换
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

        rate.sleep()

增加参考系

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

import rospy
import tf

if __name__ == '__main__':
    rospy.init_node('my_tf_broadcaster')
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        br.sendTransform((0.0, 2.0, 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")
        rate.sleep()

# 创建一个新的变换,从父”turtle1“新子”carrot1”。carrot1从turtle1偏移2米。

广播移动的参考系

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

import rospy
import tf
import math

if __name__ == '__main__':
    rospy.init_node('my_tf_broadcaster')
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        t = rospy.Time.now().to_sec() * math.pi
        br.sendTransform((2.0 * math.sin(t), 2.0 * math.cos(t), 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")
        rate.sleep()

6. 其他

(1)使用参数

<launch>
  <!-- set /global_example parameter -->
  <param name="global_example" value="global value" />
  <group ns="foo">
    <!-- set /foo/utterance -->
    <param name="utterance" value="Hello World" />
    <param name="to_delete" value="Delete Me" />
    <!-- a group of parameters that we will fetch together -->
    <group ns="gains">
      <param name="P" value="1.0" />
      <param name="I" value="2.0" />
      <param name="D" value="3.0" />      
    </group>
    <node pkg="rospy_tutorials" name="param_talker" type="param_talker.py" output="screen">
      <!-- set /foo/utterance/param_talker/topic_name -->
      <param name="topic_name" value="chatter" />
    </node>
  </group>
</launch>
#!/usr/bin/env python

import rospy
from std_msgs.msg import String

def param_talker():
    rospy.init_node('param_talker')

    # 从参数服务器获取值。在本例中,我们从三个不同的名称空间获取参数:
    # 1) global (/global_example)
    # 2) parent (/foo/utterance)
    # 3) private (/foo/param_talker/topic_name)

    # fetch a /global parameter
    global_example = rospy.get_param("/global_example")
    rospy.loginfo("%s is %s", rospy.resolve_name('/global_example'), global_example)
    
    # fetch the utterance parameter from our parent namespace
    utterance = rospy.get_param('utterance')
    rospy.loginfo("%s is %s", rospy.resolve_name('utterance'), utterance)
    
    # fetch topic_name from the ~private namespace
    topic_name = rospy.get_param('~topic_name')
    rospy.loginfo("%s is %s", rospy.resolve_name('~topic_name'), topic_name)

    # fetch a parameter, using 'default_value' if it doesn't exist
    default_param = rospy.get_param('default_param', 'default_value')
    rospy.loginfo('%s is %s', rospy.resolve_name('default_param'), default_param)
    
    # fetch a group (dictionary) of parameters
    gains = rospy.get_param('gains')
    p, i, d = gains['P'], gains['I'], gains['D']
    rospy.loginfo("gains are %s, %s, %s", p, i, d)    

    # set some parameters
    rospy.loginfo('setting parameters...')
    rospy.set_param('list_of_floats', [1., 2., 3., 4.])
    rospy.set_param('bool_True', True)
    rospy.set_param('~private_bar', 1+2)
    rospy.set_param('to_delete', 'baz')
    rospy.loginfo('...parameters have been set')

    # delete a parameter
    if rospy.has_param('to_delete'):
        rospy.delete_param('to_delete')
        rospy.loginfo("deleted %s parameter"%rospy.resolve_name('to_delete'))
    else:
        rospy.loginfo('parameter %s was already deleted'%rospy.resolve_name('to_delete'))

    # search for a parameter
    param_name = rospy.search_param('global_example')
    rospy.loginfo('found global_example parameter under key: %s'%param_name)
    
    # publish the value of utterance repeatedly
    pub = rospy.Publisher(topic_name, String, queue_size=10)
    while not rospy.is_shutdown():
        pub.publish(utterance)
        rospy.loginfo(utterance)
        rospy.sleep(1)
        
if __name__ == '__main__':
    try:
        param_talker()
    except rospy.ROSInterruptException: pass

(2)使用日志

日志按严重程度,分为:DEBUGINFOWARNERRORFATAL

  • DEBUG(调试) , 您永远不需要查看系统是否正常工作的信息
  • INFO(信息) ,少量的信息,可能是有用的用户
  • WARN(警告) , 用户可能会发现报警,并可能会影响应用程序的输出,但是该系统的预期工作的一部分
  • ERROR(错误), 一些严重的,已经错了的信息
  • FATAL(致命),事情发生了不可恢复的

日志API:

rospy.logdebug(msg, *args)
rospy.logwarn(msg, *args)
rospy.loginfo(msg, *args)
rospy.logerr(msg, *args)
rospy.logfatal(msg, *args)
#!/usr/bin/env python

import rospy
from  std_msgs.msg import String

def log_level():

    debug ="Received a message on topic X from caller Y"
    rospy.logdebug("it is debug: %s", debug)

    info = "Node initialized"
    rospy.loginfo("it is info: %s", info)

    warn = "Could not load configuration file from <path>. Using defaults"
    rospy.logwarn("it is warn: %s", warn)

    error = "Received unexpected NaN value in transform X. Skipping..."
    rospy.logerr("it is error:%s", error)

    fatal = "Motors have caught fire!"
    rospy.logfatal("it is fatal: %s", fatal)


if __name__ == '__main__':

    try:
        rospy.init_node('log_level', log_level=rospy.DEBUG)
        log_level()
    except rospy.ROSInterruptException :
        pass

(3)节点的初始化及关闭

  • 初始化节点
rospy.init_node('my_node_name')
rospy.init_node('my_node_name', anonymous=True)
rospy.init_node(name, anonymous=False, log_level=rospy.INFO, disable_signals=False) 

# anonymous=True,这样可以启动多个版本。
# log_level=rospy.INFO,日志级别。设置默认记录到rosout的信息。
# disable_signals=False,默认rospy注册信号处理器以便可以使用ctrl+c来退出。下面的情况下,你可以禁止:
	#不是从python的主线程调用init_node()。python只允许主线程注册信号处理器。
	#在wxPython或其他GUI工具运行rospy,它们有自己的退出处理。
	#你希望默认使用自己的信号处理器。
  • 节点关闭
rospy.on_shutdown(h)     #当处理关闭,会调用h函数,h函数不带参数。
rospy.signal_shutdown(reason)  # 在初始化节点,disable_signals为True时。手工处理,reason 为关闭理由,字符串内容

(4)名称和节点信息

  • rospy.get_name(),获取此节点的完全限定名称
  • rospy.get_namespace(),获取此节点的命名空间
  • rospy.get_node_uri(),获取这个节点的XMLRPC URI

(5)时间

在rospy由rospy.Time和rospy.Duration实现,前者特定时刻,后者为持续时间(可负)

  • int32 secs
  • int32 nsecs

https://www.ncnynl.com/archives/201611/1074.html

(6)异常

  • ROSException,ROS客户端基本异常类
  • ROSSerializationException,信息序列化的错误异常
  • ROSInitException,初始化ROS状态的错误异常
  • ROSInterruptException,操作中断的错误异常,经常在 rospy.sleep() and rospy.Rate 中用到
  • ROSInternalException,rospy内部错误的异常 (i.e. bugs).
  • ServiceException,ROS服务通讯相关错误的异常

参考:

  • https://www.ncnynl.com/archives/201611/1059.html
  • https://www.ncnynl.com/archives/201611/1060.html
  • https://www.ncnynl.com/archives/201611/1061.html
  • https://www.ncnynl.com/archives/201611/1062.html
  • https://www.ncnynl.com/archives/201611/1063.html
  • https://www.ncnynl.com/archives/201611/1064.html
  • https://www.ncnynl.com/archives/201611/1065.html
  • https://www.ncnynl.com/archives/201611/1066.html
  • https://www.ncnynl.com/archives/201611/1067.html
  • https://www.ncnynl.com/archives/201611/1068.html
  • https://www.ncnynl.com/archives/201611/1069.html
  • https://www.ncnynl.com/archives/201611/1070.html
  • https://www.ncnynl.com/archives/201611/1071.html
  • https://www.ncnynl.com/archives/201611/1072.html
  • https://www.ncnynl.com/archives/201611/1073.html
  • https://www.ncnynl.com/archives/201611/1074.html
  • https://www.ncnynl.com/archives/201611/1075.html
  • https://www.ncnynl.com/archives/201611/1076.html
  • https://www.ncnynl.com/archives/201611/1077.html
  • https://www.ncnynl.com/archives/201611/1078.html
  • https://www.ncnynl.com/archives/201611/1079.html
  • https://www.ncnynl.com/archives/201611/1080.html
  • https://www.ncnynl.com/archives/201611/1081.html
  • https://www.ncnynl.com/archives/201611/1082.html
  • https://www.ncnynl.com/archives/201611/1083.html
©️2020 CSDN 皮肤主题: 点我我会动 设计师:上身试试 返回首页