io_interface.py 文件源码

def publish_command(self, op, args, timeout=2.0):
        """
        publish on the command topic
        return true if the command is acknowleged within the timeout
        """
        cmd_time = rospy.Time.now()
        self.cmd_times.append(cmd_time)
        self.cmd_times = self.cmd_times[-100:]
        cmd_msg = IOComponentCommand(
            time=cmd_time,
            op=op,
            args=json.dumps(args))
        rospy.logdebug("publish_command %s %s" % (cmd_msg.op, cmd_msg.args))
        if timeout != None:
            timeout_time = rospy.Time.now() + rospy.Duration(timeout)
            while not rospy.is_shutdown():
                self._command_pub.publish(cmd_msg)
                if self.is_state_valid():
                    if cmd_time in self.state.commands:
                        rospy.logdebug("command %s acknowleged" % (cmd_msg.op,))
                        return True
                rospy.sleep(0.1)
                if timeout_time < rospy.Time.now():
                    rospy.logwarn("Timed out waiting for command acknowlegment...")
                    break
            return False
        return True