py_trees实践:实现机器人循迹任务

书接上回的py_trees快速实践，写了一个机器人沿着拓扑路径循迹移动，最后到达目标点后，执行一个任务动作的行为树。在行为树中，增加了在每个tick检查机器人电量的逻辑。在电量低于一定阈值时，会中断当前任务并触发充电动作。这个逻辑体现了行为树响应性(Reactive)的特点，希望对学习行为树的同学有一点参考价值。

下面直接给出相应的代码：

#!/usr/bin/python3
# coding: utf-8
# robot_mission_behaviors.py

from py_trees import behaviour,behaviours, blackboard, common, meta, composites, display, decorators, idioms, trees, visitors
import functools
import operator
import sys
import time
import threading

class Plan(behaviour.Behaviour):
    def __init__(self, name):
        super(Plan,self).__init__(name)
        self.blackboard = self.attach_blackboard_client()
        self.blackboard.register_key(key="goal_list", access=common.Access.WRITE)
        self.blackboard.register_key(key="new_goal_activate", access=common.Access.WRITE)
    
    def initialise(self) -> None:
        pass

    def update(self) -> common.Status:
        # path planing
        planned_path = [1,2,3,4,5,6,7,8]
        succeed = True
        self.blackboard.goal_list = planned_path
        if succeed:
            self.feedback_message = "path plan succeed."
            self.blackboard.new_goal_activate = True
            return common.Status.SUCCESS
        else:
            self.feedback_message = "path plan failed!"
            self.blackboard.new_goal_activate = False
            return common.Status.FAILURE
        
class SendNavGoal(behaviour.Behaviour):
    def __init__(self, name):
        super(SendNavGoal, self).__init__(name)
        self.blackboard = self.attach_blackboard_client()
        self.blackboard.register_key(key="new_goal_activate", access=common.Access.WRITE)
        self.blackboard.register_key(key="goal_list", access=common.Access.READ)
        self.blackboard.register_key(key="nav_return_code", access=common.Access.WRITE)
    
    def initialise(self) -> None:
        pass
    
    def send_nav_goal(self,callback):
        # send nav_point
        pass

    def nav_gaol_cb(self,error_code):
        self.blackboard.nav_return_code = error_code

    def update(self) -> common.Status:
        if self.blackboard.new_goal_activate and self.blackboard.goal_list:
            goal = self.blackboard.goal_list.pop(0)
            print("-*-*- Go to path: {}".format(goal))
            #self.thread = threading.Thread(target=self.send_nav_goal,args=(self.nav_gaol_cb))
            #self.thread.setDaemon(True)
            #self.thread.start()
            #self.blackboard.new_goal_activate = False
            #self.feedback_message = "send nav goal."
        return common.Status.SUCCESS
    
class CheckPowerStatus(behaviour.Behaviour):
    def __init__(self, name):
        super(CheckPowerStatus, self).__init__(name)
        self.count = 3

    def initialise(self) -> None:
        pass

    def update(self) -> common.Status:
        # check power status
        if self.count == 0:
            power_ok = False
        else:
            power_ok = True
            self.count -= 1
        if power_ok:
            return common.Status.SUCCESS
        else:
            return common.Status.FAILURE
        
class SetGoalToCharge(behaviour.Behaviour):
    def __init__(self, name):
        super(SetGoalToCharge,self).__init__(name)
        self.blackboard = self.attach_blackboard_client()
        self.blackboard.register_key(key="goal_list", access=common.Access.WRITE)
        self.blackboard.register_key(key="new_goal_activate", access=common.Access.WRITE)
    
    def initialise(self) -> None:
        pass
    
    def update(self) -> common.Status:
        # charge re-plan
        self.blackboard.goal_list = [100,99,98,97,96,95]
        return common.Status.SUCCESS


class QueryNavStatus(behaviour.Behaviour):
    def __init__(self, name):
        super(QueryNavStatus, self).__init__(name)
        self.b