action_manager.py 文件源码

def run(self):
        """Send an action at a 40Hz cycle."""
        rospy.init_node('action_manager', anonymous=True)
        rospy.Subscriber('action_cmd', Twist, self.update_action)
        rospy.Subscriber('termination', Bool, self.set_termination_flag)
        rospy.Subscriber('pause', Bool, self.set_pause_flag)

        action_publisher = rospy.Publisher('cmd_vel_mux/input/teleop',
                                           Twist,
                                           queue_size=1)

        action_pub_rate = rospy.Rate(40)

        while not rospy.is_shutdown():
            if self.termination_flag:
                break
            if self.pause_flag is False:
                # log action
                speeds = (self.action.linear.x, self.action.angular.z)
                actn = "linear: {}, angular: {}".format(*speeds)
                rospy.logdebug("Sending action to Turtlebot: {}".format(actn))

                # send new actions
                if self.stop_once:
                    action_publisher.publish(self.STOP_ACTION)
                    self.stop_once = False
                else:
                    action_publisher.publish(self.action)
            action_pub_rate.sleep()