add examples for trajectory generators

air_navigation_examples/__init__.py

@@ -4,32 +4,72 @@ import sys
 class TestController(air_navigation.Controller):
   def __init__(self): 
     super().__init__()
-    self.print("Constructed")
   
   def configure(self, name):
-    self.print("Configured " + name)
+    pass
 
   def cleanup(self):
-    self.print("Cleanedup")
+    pass
 
   def activate(self):
-    self.print("Activated")
+    pass
 
   def deactivate(self):
-    self.print("Deactivated")
+    pass
 
-  def computeVelocityCommands(self, pose, velocity):
-    # self.print("computeVelocityCommands")
+  def computeVelocityCommands(self, pose, velocity, goal):
     t = air_navigation.TwistStamped()
     t.twist.linear.x = 1.0
     t.twist.angular.z = 2.0
-    # self.print("computeVelocityCommands" + str(t))
     return t
 
   def setPlan(self, path):
-    self.print("setPlan")
     pass
 
   def setSpeedLimit(self, speed_limit, percentage):
-    self.print("setSpeedLimit")
     pass
+
+class TrajectoryCritic(air_navigation.TrajectoryCritic):
+  def __init__(self):
+    super().__init__()
+  
+  def onInit(self):
+    pass
+
+  def reset(self):
+    pass
+
+  def prepare(self, pose, vel, goal, global_plan):
+    return False
+
+  def scoreTrajectory(self, traj):
+    return 1.0
+
+  def debrief(self, vel):
+    pass
+
+class TrajectoryGenerator(air_navigation.TrajectoryCritic):
+  def __init__(self):
+    super().__init__()
+  
+  def initialize(self, name):
+    pass
+
+  def reset(self):
+    pass
+
+  def startNewIteration(self, current_velocity):
+    pass
+
+  def hasMoreTwists(self):
+    return False
+
+  def nextTwist(self):
+    return air_navigation.Twist2D()
+
+  def generateTrajectory(self, start_pose, start_vel, cmd_vel):
+    return air_navigation.Trajectory2D()
+
+  def setSpeedLimit(self, speed_limit, percentage):
+    pass
+    
\ No newline at end of file