0

我有一个具有两个功能的代码。函数'send_thread'和函数'receive_thread'是'send_thread'的回调。我想要做的是运行'send_thread'，这会激活'receive_thread'，一旦结束，再次重复。为此，我想出了下面的代码。这并没有给出预期的结果，因为“send_thread”被再次调用但不再激活回调。预先感谢您的帮助。

我注意到，该函数在 receive_thread 结束时被调用，并运行了我在 send_thread (rospy.sleep()) 中等待的时间。不过，在第一次尝试后，我再也不会激活回调。

import rospy
import pepper_2d_simulator
import threading

class TROS(object):
    def __init__(self):
        self.cmd_vel_pub = rospy.Publisher('cmd_vel',Twist)
        self.event = threading.Event()

    def send_thread(self):
        #send commmand
        self.event.set()
        sequence = [[1,0,0.05],[0,0,0],[0,0,0.1292]]
        for cmd in sequence:
            rospy.Rate(0.5).sleep()
            msg = Twist()
            msg.linear.x = cmd[0]
            msg.linear.y = cmd[1]
            msg.angular.z = cmd[2]
            t = rospy.get_rostime()
            self.cmd_vel_pub.publish(msg)
        self.event.clear()
        rospy.sleep(1)

    def receive_thread(self,msg):
        #if something is being send, listen to this
        if self.event.isSet():
            frame_id = msg.header.frame_id
            self.x_odom = msg.pose.pose.position.x
            self.y_odom = msg.pose.pose.position.y
            self.z_odom = msg.pose.pose.position.z
            self.pos_odom = [self.x_odom,self.y_odom,self.z_odom,1]
            self.ang_odom = msg.pose.pose.orientation.z
            self.time = msg.header.stamp.secs + msg.header.stamp.nsecs 
            #some transformations here to get self.trans...         
        else:
            #after self.event() is cleared, rename and run again
            self.x_odom = self.trans_br_x
            self.y_odom = self.trans_br_y
            self.ang_odom = self.rot_br_ang
            self.send_thread()

    def init_node(self):
        rospy.init_node('pepper_cmd_evaluator',anonymous = True)                   
        rospy.Subscriber('odom',Odometry,self.receive_thread)

if __name__ == '__main__':
    thinking = Thinking()
    thinking.init_node()
    thinking.send_thread()

The expected result is that I am able to loop this two function so that I call send_thread, this activates receive thread. Then send_thread stops, receive_thread stops and activates the send_thread again. I want to do this 10 times.
4

1 回答 1

0

我现在已经弄清楚了如何做到这一点。如果其他人遇到类似问题，我将发布我的解决方案。我想出的工作解决方案非常简单。我创建了一个 self.flag 变量，并在 send_thread 和 callback 中分别将其设置为 True 和 False。编码：

import rospy
import pepper_2d_simulator
import threading

class TROS(object):
    def __init__(self):
        self.cmd_vel_pub = rospy.Publisher('cmd_vel',Twist)
        self.event = threading.Event()
        self.count = 0
        self.flag = True

    def send_thread(self):
        while self.count < 10:
            if self.flag:
                self.count = self.count + 1
                #send commmand
                self.event.set()
                sequence = [[1,0,0.05],[0,0,0],[0,0,0.1292]]
                for cmd in sequence:
                    rospy.Rate(0.5).sleep()
                    msg = Twist()
                    msg.linear.x = cmd[0]
                    msg.linear.y = cmd[1]
                    msg.angular.z = cmd[2]
                    t = rospy.get_rostime()
                    self.cmd_vel_pub.publish(msg)
                self.event.clear()
                rospy.sleep(0.3)
                self.flag = False
        rospy.signal_shutdown('Command finished')

    def receive_thread(self,msg):
        #if something is being send, listen to this
        if self.event.isSet():
            frame_id = msg.header.frame_id
            self.x_odom = msg.pose.pose.position.x
            self.y_odom = msg.pose.pose.position.y
            self.z_odom = msg.pose.pose.position.z
            self.pos_odom = [self.x_odom,self.y_odom,self.z_odom,1]
            self.ang_odom = msg.pose.pose.orientation.z
            self.time = msg.header.stamp.secs + msg.header.stamp.nsecs 
            #some transformations here to get self.trans...         
        else:
            #after self.event() is cleared, rename and run again
            self.x_odom = self.trans_br_x
            self.y_odom = self.trans_br_y
            self.ang_odom = self.rot_br_ang
            self.flag = True

    def init_node(self):
        rospy.init_node('pepper_cmd_evaluator',anonymous = True)                   
        rospy.Subscriber('odom',Odometry,self.receive_thread)

if __name__ == '__main__':
    thinking = Thinking()
    thinking.init_node()
    thinking.send_thread()
于 2019-07-23T14:25:17.463 回答