lqrrt_node.py 文件源码

def __init__(self, odom_topic, ref_topic, move_topic, path_topic, tree_topic,
                 goal_topic, focus_topic, effort_topic, ogrid_topic, ogrid_threshold):
        """
        Initialize with topic names and ogrid threshold as applicable.
        Defaults are generated at the ROS params level.

        """
        # One-time initializations
        self.revisit_period = 0.05  # s
        self.ogrid = None
        self.ogrid_threshold = float(ogrid_threshold)
        self.state = None
        self.tracking = None
        self.done = True

        # Lil helpers
        self.rostime = lambda: rospy.Time.now().to_sec()
        self.intup = lambda arr: tuple(np.array(arr, dtype=np.int64))
        self.get_hood = lambda img, row, col: img[row-1:row+2, col-1:col+2]

        # Set-up planners
        self.behaviors_list = [car, boat, escape]
        for behavior in self.behaviors_list:
            behavior.planner.set_system(erf=self.erf)
            behavior.planner.set_runtime(sys_time=self.rostime)
            behavior.planner.constraints.set_feasibility_function(self.is_feasible)

        # Initialize resetable stuff
        self.reset()

        # Subscribers
        rospy.Subscriber(odom_topic, Odometry, self.odom_cb)
        rospy.Subscriber(ogrid_topic, OccupancyGrid, self.ogrid_cb)
        rospy.sleep(0.5)

        # Publishers
        self.ref_pub = rospy.Publisher(ref_topic, Odometry, queue_size=1)
        self.path_pub = rospy.Publisher(path_topic, PoseArray, queue_size=3)
        self.tree_pub = rospy.Publisher(tree_topic, PoseArray, queue_size=3)
        self.goal_pub = rospy.Publisher(goal_topic, PoseStamped, queue_size=3)
        self.focus_pub = rospy.Publisher(focus_topic, PointStamped, queue_size=3)
        self.eff_pub = rospy.Publisher(effort_topic, WrenchStamped, queue_size=3)
        self.sampspace_pub = rospy.Publisher(sampspace_topic, PolygonStamped, queue_size=3)
        self.guide_pub = rospy.Publisher(guide_topic, PointStamped, queue_size=3)

        # Actions
        self.move_server = actionlib.SimpleActionServer(move_topic, MoveAction, execute_cb=self.move_cb, auto_start=False)
        self.move_server.start()
        rospy.sleep(1)

        # Timers
        rospy.Timer(rospy.Duration(self.revisit_period), self.publish_ref)
        rospy.Timer(rospy.Duration(self.revisit_period), self.action_check)