Added evaluation script

.gitignore

@@ -1,3 +1,5 @@
+evaluation.yaml
+envtest/ros/summary.yaml
 
 # Python stuff
 *.pyc

envtest/ros/evaluation_config.yaml

@@ -6,6 +6,8 @@ topics:
   finish: "finish_navigation"
 
 target: 60 # must be INTEGER
+timeout: 30
+bounding_box: [-5, 65, -10, 10, 0, 10]
 
 # if you really don't want the cool plots, put this to False
 plots: True

envtest/ros/evaluation_node.py

@@ -25,29 +25,32 @@ def __init__(self, config):
 
         self.hit_obstacle = False
         self.crash = 0
-
+        self.timeout = self.config['timeout']
+        self.bounding_box = np.reshape(np.array(
+            self.config['bounding_box'], dtype=float), (3,2)).T
+
         self._initSubscribers(config['topics'])
         self._initPublishers(config['topics'])
 
 
     def _initSubscribers(self, config):
         self.state_sub = rospy.Subscriber(
                 "/%s/%s" % (config['quad_name'], config['state']),
-                QuadState, 
+                QuadState,
                 self.callbackState,
                 queue_size=1,
                 tcp_nodelay=True)
-        
+
         self.obstacle_sub = rospy.Subscriber(
                 "/%s/%s" % (config['quad_name'], config['obstacles']),
-                ObstacleArray, 
+                ObstacleArray,
                 self.callbackObstacles,
                 queue_size=1,
                 tcp_nodelay=True)
 
         self.start_sub = rospy.Subscriber(
                 "/%s/%s" % (config['quad_name'], config['start']),
-                Empty, 
+                Empty,
                 self.callbackStart,
                 queue_size=1,
                 tcp_nodelay=True)
@@ -70,12 +73,11 @@ def callbackState(self, msg):
         if not self.is_active:
             return
 
-        self.pos.append(np.array(
-                        [msg.header.stamp.to_sec(),
-                         msg.pose.position.x,
-                         msg.pose.position.y,
-                         msg.pose.position.z]
-                        ))
+        pos = np.array([msg.header.stamp.to_sec(),
+                        msg.pose.position.x,
+                        msg.pose.position.y,
+                        msg.pose.position.z])
+        self.pos.append(pos)
 
         pos_x = msg.pose.position.x
         bin_x = int(max(min(np.floor(pos_x),self.xmax),0))
@@ -86,38 +88,76 @@ def callbackState(self, msg):
             self.is_active = False
             self.publishFinish()
 
+        if rospy.get_time() - self.time_array[0] > self.timeout:
+            self.abortRun()
+
+        outside = ((pos[1:] > self.bounding_box[1,:])
+                    | (pos[1:] < self.bounding_box[0,:])).any(axis=-1)
+        if (outside == True).any():
+            self.abortRun()
+
+
 
     def callbackStart(self, msg):
-        self.is_active = True
+        if not self.is_active:
+            self.is_active = True
         self.time_array[0] = rospy.get_rostime().to_sec()
 
 
     def callbackObstacles(self, msg):
         if not self.is_active:
             return
-        
+
         obs = msg.obstacles[0]
-        dist = np.linalg.norm(np.array([obs.position.x, 
-                                        obs.position.y, 
+        dist = np.linalg.norm(np.array([obs.position.x,
+                                        obs.position.y,
                                         obs.position.z]))
         margin = dist - obs.scale/2
         self.dist.append([msg.header.stamp.to_sec(), margin])
         if margin < 0:
             if not self.hit_obstacle:
                 self.crash += 1
+                print("Crashed")
             self.hit_obstacle = True
         else:
             self.hit_obstacle = False
 
 
+    def abortRun(self):
+        print("You did not reach the goal!")
+        summary = {}
+        summary['Success'] = False
+        with open("summary.yaml", "w") as f:
+            if os.getenv('ROLLOUT_NAME') is not None:
+                tmp = {}
+                tmp[os.getenv('ROLLOUT_NAME')] = summary
+                yaml.safe_dump(tmp, f)
+            else:
+                yaml.safe_dump(summary, f)
+        rospy.signal_shutdown("Completed Evaluation")
+
+
     def printSummary(self):
         ttf = self.time_array[-1] - self.time_array[0]
+        summary = {}
+        summary['Success'] = True if self.crash == 0 else False
         print("You reached the goal in %5.3f seconds" % ttf)
+        summary['time_to_finish'] = ttf
         print("Your intermediate times are:")
         print_distance = 10
-        for i in range(print_distance, self.xmax-print_distance+1, print_distance):
+        summary['segment_times'] = {}
+        for i in range(print_distance, self.xmax+1, print_distance):
             print("    %2i: %5.3fs " % (i,self.time_array[i] - self.time_array[0]))
+            summary['segment_times']["%i" % i] = self.time_array[i] - self.time_array[0]
         print("You hit %i obstacles" % self.crash)
+        summary['number_crashes'] = self.crash
+        with open("summary.yaml", "w") as f:
+            if os.getenv('ROLLOUT_NAME') is not None:
+                tmp = {}
+                tmp[os.getenv('ROLLOUT_NAME')] = summary
+                yaml.safe_dump(tmp, f)
+            else:
+                yaml.safe_dump(summary, f)
 
         if not self.config['plots']:
             return
@@ -131,11 +171,13 @@ def printSummary(self):
         dt = np.array(self.time_array)
         y = 1/(dt[1:]-dt[0:-1])
         plot(xs=x, ys=y, color=True)
-        
+
         print("Here is a plot of the distance to the closest obstacles")
         dist = np.array(self.dist)
         plot(xs=dist[:,0]-self.time_array[0], ys=dist[:,1], color=True)
 
+        rospy.signal_shutdown("Completed Evaluation")
+
 
 
 if __name__=="__main__":

launch_evaluation.bash

@@ -0,0 +1,64 @@
+#!/bin/bash
+
+# Pass number of rollouts as argument
+if [ $1 ]
+then
+  N="$1"
+else
+  N=10
+fi
+
+# Set Flightmare Path if it is not set
+if [ -z $FLIGHTMARE_PATH ]
+then
+  export FLIGHTMARE_PATH=$PWD/flightmare
+fi
+
+# Launch the simulator, unless it is already running
+if [ -z $(pgrep visionsim_node) ]
+then
+  roslaunch envsim visionenv_sim.launch render:=True &
+  ROS_PID="$!"
+  echo $ROS_PID
+  sleep 10
+else
+  ROS_PID=""
+fi
+
+SUMMARY_FILE="evaluation.yaml"
+"" > $SUMMARY_FILE
+
+# Perform N evaluation runs
+for i in $(eval echo {1..$N})
+do
+  # Publish simulator reset
+  rostopic pub /kingfisher/agiros_pilot/off std_msgs/Empty "{}" --once
+  rostopic pub /kingfisher/agiros_pilot/reset_sim std_msgs/Empty "{}" --once
+  rostopic pub /kingfisher/agiros_pilot/enable std_msgs/Bool "data: true" --once
+
+  export ROLLOUT_NAME="rollout_""$i"
+  echo "$ROLLOUT_NAME"
+
+  cd ./envtest/ros/
+  python evaluation_node.py &
+  PY_PID="$!"
+  cd -
+
+  sleep 0.5
+  rostopic pub /kingfisher/start_navigation std_msgs/Empty "{}" --once
+
+  # Wait until the evaluation script has finished
+  while ps -p $PY_PID > /dev/null
+  do
+    sleep 1
+  done
+
+  cat "$SUMMARY_FILE" "./envtest/ros/summary.yaml" > "tmp.yaml"
+  mv "tmp.yaml" "$SUMMARY_FILE"
+done
+
+
+if [ $ROS_PID ]
+then
+  kill -SIGINT "$ROS_PID"
+fi