Mission monitor sequencer

Introduction

This plug-in implements a a language that is a mixture of a rule-based and a sequential based language. The idea is that a number of situations need permanent - or semi permanent - monitoring to get a good situation awareness for the root, and at the same time a large number of the robot tasks are better described by a sequential language.

This implementation attempts to cover this gab.

Language

An example function could look like this:

<?xml version="1.0" ?>
<plan name="CrossRoad">
   <init>
       odoPose.tripB = 0
       nearRoad = false // define local variable
       <plan name="maxOdoDist" if="odoPose.tripB > 250"> 
           print("Driven too far on odo " odoPose.tripB "m") // print message
           break CrossRoad // failed to cross road - could trigger a relocalization
       </plan>
       <plan name="closeToRoad" if="hypot(utmPose.poseY - 6174307, utmPose.poseX - 707873) < 15" >
           print("Cose to road slowing down) // print message
           smr.speed=0.5  // set the desired maximum speed in mrc interface module
           nearRoad = true  // set flag
           disable  // disable this rule (plan)
       </plan>
       <plan name="turn">
           <parameter angle="pi" dist=1.0/>
           <commands to="smr.send">
             # construct commands to MRC using the smr.send command
             'drive @v ' smr.speed ' : ($drivendist > ' dist ')'
             'turn ' angle
             'drive : ($drivendist > ' dist ')'
           </commands>
       </plan>
   </init>
   print("started")
   enable("maxOdoDist")   // should have found road by now
   enable("closeToRoad")  // active until disabled.
   roaddrive.right(0.75) : nearRoad // follow road 75cm from edge until near road
   // more stuff missing here to detect traffic etc.
   ...
   turn() // turn back - using a call to a plan
   success=true
   <post>
       print("finsihed crossRoad - success=" success)
   </post>
</plan>

A number of these plans can be started (enabled) simultaniously, an can in principle be total independant. I.e.one can control the robot arm whele another controls the navigation.