How to shutdown a blocked rospy program

asked 2016-04-07 15:41:37 -0500

lightbulb gravatar image

Hello all,

I have a problem in dealing with my rospy program. I use REQ-REP method in ZMQ to handle message from client in Python. But the problem is that socket.recv() will block program, so that the rospy.is_shutdown() will not be reached. For that, i can't shutdown my program manually. Please help me.

#!/usr/bin/env python
import rospy 
import geometry_msgs.msg
from math import *
import zmq
import time
import json
import signal

def _zmq_init():
    global context
    global socket
    context = zmq.Context()
    socket = context.socket(zmq.REP)

def _message_init():
    global pub
    global msg
    pub = rospy.Publisher('tele_pos', geometry_msgs.msg.PoseStamped, queue_size = 10)
    rospy.init_node('tele_pos', anonymous = False)
    msg = geometry_msgs.msg.PoseStamped()

def tele_pos():
    while not rospy.is_shutdown():
        global seq
        global degree
        global message
        global pub
        global msg
        message = socket.recv()
        d = json.loads(message)
        _degree = d['yaw'] / 2
        sin_degree = sin(radians(_degree))
        cos_degree = cos(radians(_degree))
        msg.header.seq = seq
        msg.header.stamp =
        msg.header.frame_id = 'base_footprint'
        msg.pose.position.x = d['x']
        msg.pose.position.y = d['y']
        msg.pose.position.z = d['z']
        msg.pose.orientation.x = 0.0
        msg.pose.orientation.y = 0.0
        msg.pose.orientation.z = sin_degree
        msg.pose.orientation.w = cos_degree
        seq += 1

if __name__ == '__main__':
        seq = 1
        degree = 0.0
    except rospy.ROSInterruptException:
edit retag flag offensive close merge delete