How to shutdown a blocked rospy program

edit

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)
    socket.bind("tcp://*:5555")

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 = rospy.Time.now()
        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
        pub.publish(msg)
        socket.send('OK')

if __name__ == '__main__':
    try:
        seq = 1
        degree = 0.0
        _zmq_init()
        _message_init()
        tele_pos()
    except rospy.ROSInterruptException:
        pass