ROS 进阶学习笔记（13） - Combine Subscriber and Publisher in Python, ROS

Combine Subscriber and Publisher in Python, ROS

This article will describe an example of Combining Subscriber and Publisher in Python in ROS programming.

This is very useful in ROS development.

We will also discuss briefly how to build and modify a catkin package which is written by Python.

• Create a catkin package with the command: catkin_create_pkg, under the path: ~/catkin_ws/src
• Build it with the command: catkin_make, under the path: ~/catkin_ws/
• Source the catkin setup file under devel folder:
  

  $ source ~/catkin_ws/devel/setup.bash

• modify the Python scripts file under the path: ~/catkin_ws/src/<pkg_name>/scripts/nodexxx.py
• chmod +x nodexxx.py
• Run this package by Command:  rosrun package_name nodexxx.py
• Modify the CMakefile.txt for Python: Writing a ROS Python Makefile
  

More about Create and Build catkin ROS package: This blog

The sourcecode for this Combining Subscriber and Publisher in Python is here:

#!/usr/bin/env python
# License removed for brevity
"""
	learn to write Subscriber and Listener in one python script.
	Function Style
	Author: Sonic http://blog.csdn.net/sonictl
	Date: Feb 29, 2016	
"""
import rospy
from std_msgs.msg import String

def callback(data):
    rospy.loginfo(rospy.get_caller_id() + "callback:I heard %s", data.data)
    #resp_str = "resp_str: I heard: " + data.data
    talker(data)

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously. http://blog.csdn.net/sonictl
    rospy.init_node('responsor', anonymous=True)

    rospy.Subscriber("uc0Response", String, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()
	
def talker(data):
    pub = rospy.Publisher('uc0Command', String, queue_size=10)
    rospy.loginfo("talker:I heard %s", data.data)

    #while not rospy.is_shutdown():
    resp_str = "resp_str: I heard: " + data.data
    rospy.loginfo(resp_str)
    if data.data == "cigit-pc\n" :
        pub.publish(resp_str)
    else:
        rospy.loginfo("invalid seri data:" + data.data)
		
if __name__ == '__main__':
    #listener()
    try:
        listener()
        #talker()
    except rospy.ROSInterruptException:
        pass

20160614: I have to paste the next code for an other instance because this should be very good for learners:

#!/usr/bin/env python

""" odom_ekf.py - Version 0.1 2012-07-08
    Republish the /robot_pose_ekf/odom_combined topic which is of type 
    geometry_msgs/PoseWithCovarianceStamped as an equivalent message of
    type nav_msgs/Odometry so we can view it in RViz.
    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2012 Patrick Goebel.  All rights reserved.
    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.5
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
      
"""

import roslib; roslib.load_manifest('rbx1_nav')
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped
from nav_msgs.msg import Odometry

class OdomEKF():
    def __init__(self):
        # Give the node a name
        rospy.init_node('odom_ekf', anonymous=False)

        # Publisher of type nav_msgs/Odometry
        self.ekf_pub = rospy.Publisher('output', Odometry)
        
        # Wait for the /odom topic to become available
        rospy.wait_for_message('input', PoseWithCovarianceStamped)
        
        # Subscribe to the /robot_pose_ekf/odom_combined topic
        rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
        
        rospy.loginfo("Publishing combined odometry on /odom_ekf")
        
    def pub_ekf_odom(self, msg):
        odom = Odometry()
        odom.header = msg.header
        odom.child_frame_id = 'base_footprint'
        odom.pose = msg.pose
        
        self.ekf_pub.publish(odom)
        
if __name__ == '__main__':
    try:
        OdomEKF()
        rospy.spin()
    except:
        pass
        

This program above is for Transfer a PoseWithCovarianceStamped typed topic "input" into Odometry typed topic which is named "output".
We can learn how to initialize a node, declare a publisher, subscriber and how to use wait_for_message() function. The callback function pub_ekf_odom is called when input has data, and we can see how this function works to convert the data from PoseWithCovarianceStamped into Odometry.