IR pose estimation

config/gate_locations_no_perturbation.yaml

@@ -0,0 +1,70 @@
+Gate1:
+  location: [[-0.009051204, -34.8755, 3.071861], [-0.008949876, -32.9505, 3.071861], [-0.008949876, -32.9505, 1.134362], [-0.009051204, -34.8755, 1.134362]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate13:
+  location: [[1.237332, 9.001728, 2.9625], [3.162332, 9.001728, 2.9625], [3.162332, 9.001728, 1.025], [1.237332, 9.001728, 1.025]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate3:
+  location: [[-0.3703381, -12.99827, 4.728], [-0.3699976, -11.45827, 4.728], [-0.3699976, -11.45827, 3.177999], [-0.3703381, -12.99827, 3.177999]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate4:
+  location: [[-31.56487, -13.929, 6.99714], [-30.04735, -13.929, 7.259325], [-29.78347, -13.929, 5.731955], [-31.30098, -13.929, 5.469769]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate5:
+  location: [[-20.94317, 14.607, 2.515013], [-20.94283, 16.147, 2.515013], [-20.94283, 16.147, 0.965013], [-20.94317, 14.607, 0.965013]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate7:
+  location: [[16.2506, 41.63, 11.32092], [17.79061, 41.63, 11.32092], [17.79061, 41.63, 9.770914], [16.2506, 41.63, 9.770914]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate8:
+  location: [[-3.430186, 11.79953, 6.122593], [-3.430186, 12.97997, 6.122593], [-3.430186, 11.79952, 4.911986], [-3.430186, 12.97997, 4.911986]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate9:
+  location: [[-6.40867, -12.13678, 4.152941], [-8.208672, -12.13678, 4.152941], [-6.408669, -12.13678, 2.306941], [-8.208672, -12.13678, 2.306941]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate10:
+  location: [[18.15111, 3.631447, 7.229498], [16.35111, 3.63155, 7.229498], [18.15111, 3.631447, 5.383497], [16.35111, 3.63155, 5.383497]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate11:
+  location: [[-27.01367, 2.543159, 7.515942], [-28.81367, 2.543289, 7.515942], [-27.01367, 2.543159, 5.669941], [-28.81367, 2.543289, 5.669941]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate12:
+  location: [[18.40823, 13.60605, 9.143385], [16.33437, 14.09245, 9.143507], [18.90706, 15.73291, 9.14324], [16.83319, 16.21931, 9.143364]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate23:
+  location: [[-9.328671, 7.773174, 2.942941], [-11.12867, 7.773277, 2.942941], [-9.328669, 7.773174, 1.096941], [-11.12867, 7.773277, 1.096941]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate6:
+  location: [[-9.14867, 30.62316, 3.820941], [-10.94867, 30.62329, 3.820941], [-9.148668, 30.62316, 1.974941], [-10.94867, 30.62329, 1.974941]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate2:
+  location: [[0.8248205, 27.86797, 4.16025], [3.834821, 27.86797, 4.16025], [3.834821, 27.86797, 0.9327497], [0.8248205, 27.86797, 0.9327497]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate14:
+  location: [[-8.674376, -29.53623, 4.25625], [-6.545984, -27.40784, 4.25625], [-6.545984, -27.40784, 1.02875], [-8.674376, -29.53623, 1.02875]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate15:
+  location: [[5.744822, -11.79203, 4.16025], [8.75482, -11.79203, 4.16025], [8.75482, -11.79203, 0.9327497], [5.744822, -11.79203, 0.9327497]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate16:
+  location: [[-13.73218, 28.64197, 12.65825], [-13.73218, 26.23396, 12.65825], [-13.73218, 26.23396, 10.07625], [-13.73218, 28.64197, 10.07625]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate17:
+  location: [[-20.57, -13.29803, 3.46425], [-20.57, -15.70603, 3.46425], [-20.57, -15.70603, 0.8822498], [-20.57, -13.29803, 0.8822498]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate18:
+  location: [[13.51782, 21.76397, 3.61825], [13.51782, 19.35596, 3.61825], [13.51782, 19.35596, 1.03625], [13.51782, 21.76397, 1.03625]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate19:
+  location: [[15.96136, -17.26184, 12.31488], [18.36892, -17.26184, 12.36067], [18.41802, -17.26184, 9.779139], [16.01046, -17.26184, 9.733349]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate20:
+  location: [[-25.39118, 25.019, 7.856133], [-22.98318, 25.019, 7.856133], [-22.98318, 25.019, 5.274133], [-25.39118, 25.019, 5.274133]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate21:
+  location: [[15.68563, 38.50578, 8.376249], [17.81402, 40.63416, 8.376249], [17.81402, 40.63416, 5.14875], [15.68563, 38.50578, 5.14875]]
+  perturbation: [0.0, 0.0, 0.0]
+Gate22:
+  location: [[4.803625, -27.37584, 4.18125], [6.932016, -29.50423, 4.18125], [6.932016, -29.50423, 0.9537499], [4.803625, -27.37584, 0.9537499]]
+  perturbation: [0.0, 0.0, 0.0]
+

launch/sim.launch

@@ -4,6 +4,8 @@
   <arg name="render_stereo" default="0" />
   <arg name="use_ir_markers" default="false" />
 
+  <rosparam command="load" file="$(find simcontrol)/config/gate_locations_no_perturbation.yaml"/>
+
   <group ns="/control_nodes">
     <!-- Start human teleoperation nodes -->
     <node name="joy" pkg="joy" type="joy_node"/>
@@ -28,7 +30,7 @@
   <!--For teleoperation, run dynamics in real time-->
   <param name="/use_sim_time" value="false" />
 
-  <!-- <node pkg="simcontrol" name="publish_state" type="state_estimation.py" output="screen" /> -->
+  <node pkg="simcontrol" name="publish_state" type="state_estimation.py" output="screen" />
 
   <rosparam command="load" file="$(find simcontrol)/config/basic.yaml"/>
 

requirements.txt

@@ -1,5 +1,4 @@
 numpy-quaternion==2019.2.20.16.38.5
 simple-pid==0.1.5
 numpy==1.16.0
-
-
+opencv-python==4.0.0.21

scripts/control.py

@@ -10,13 +10,14 @@
 from collections import defaultdict
 import numpy as np
 import quaternion
-import math_utils
+from lib import math_utils
+from std_msgs.msg import String
 from simple_pid import PID
 from mav_msgs.msg import RateThrust
 from sensor_msgs.msg import Imu
 from geometry_msgs.msg import Vector3, Quaternion, Pose
 from flightgoggles.msg import IRMarkerArray, IRMarker
-from lib.graphix import camera_ray, ray_p_dist
+from lib.vision import camera_ray, ray_p_dist
 from simcontrol.msg import State
 
 GRAVITY = 9.81
@@ -34,25 +35,19 @@ def __init__(self, rate, use_gt_state=False):
         self.ir_beacons = rospy.Subscriber('/uav/camera/left/ir_beacons', IRMarkerArray, self._ir_callback)
         self.target_pub = rospy.Publisher('/simcontrol/target_pose', Pose, queue_size=1)
         self.state_pub = rospy.Publisher('/simcontrol/state_estimate', State, queue_size=1)
+        self.gate_pub = rospy.Publisher('/simcontrol/target_gate', String, queue_size=1)
 
-        self.imu_topic = '/uav/sensors/imu'
-        self.imu_subscriber = rospy.Subscriber(self.imu_topic, Imu, self._imu_callback)
         self.tf_listener = tf.TransformListener()
         self.tf_prev_x = self.init_position
         self.tf_prev_xdot = np.array([0.0, 0.0, 0.0])
         self.tf_prev_xddot = np.array([0.0, 0.0, 0.0])
-        self.imu_latest_x = self.init_position
-        self.imu_latest_xdot = np.array([0.0, 0.0, 0.0])
-        self.imu_latest_xddot = np.array([0.0, 0.0, 0.0])
-        self.imu_latest_omega = np.array([0.0, 0.0, 0.0])
         self.is_armed = False
 
         self.gate_names = rospy.get_param("/gate_names") if rospy.has_param('/gate_names') else None
         self.use_ir_markers = rospy.get_param("/use_ir_markers") if rospy.has_param('/use_ir_markers') else True
         self.target_gate = 0
         self.target_lead_distance = 5.0
-        self.reference_lead_steps = 2
-        self.target_update_weight = 1.0
+        self.target_update_weight = 0.1
         self.target_vector = np.float32([0, 0, 0])
         self.prev_tf_time = rospy.Time.now()
 
@@ -86,9 +81,6 @@ def linear_interpolate(self, p, n):
     def get_gate_info(self):
         for gate in range(1, 24):
             nominal_location = np.float32(rospy.get_param('/uav/Gate%d/nominal_location' % gate))
-            self.gate_markers['Gate%d' % gate] = {}
-            for i in range(1, 5):
-                self.gate_markers['Gate%d' % gate][i] = nominal_location[i-1,:]
             self.gate_normal['Gate%d' % gate] = -np.cross(nominal_location[1,:] - nominal_location[0,:], nominal_location[2,:] - nominal_location[0,:])
             self.gate_normal['Gate%d' % gate] /= np.linalg.norm(self.gate_normal['Gate%d' % gate])
             perturbation_bound = np.float32(rospy.get_param('/uav/Gate%d/perturbation_bound' % gate))
@@ -110,7 +102,8 @@ def get_gate_info(self):
 
         n = 10
         ev, total = self.linear_interpolate(p, n)
-        self.path = ev[:total, :]
+        self.reference_waypoints = p
+        self.reference_trajectory = ev[:total, :]
 
     def get_init_pose(self):
         if not rospy.has_param('/uav/flightgoggles_uav_dynamics/init_pose'):
@@ -128,10 +121,11 @@ def loop(self):
         rate = rospy.Rate(self.rate)
         while not rospy.is_shutdown():
             target_gate_name = self.gate_names[self.target_gate]
+            self.gate_pub.publish(String(data=target_gate_name))
             state_estimate = self.state_estimate()
             x = state_estimate[0]
             q_b = state_estimate[3]
-            new_target_vector = self.reference_target_vector(x)
+            new_target_vector = self._gaze_direction(x)
             # We only want to use IR markers to correct for gate perturbation
             # If we are close to the target gate or there is no perturbation, just use reference trajectory
             if self.use_ir_markers:
@@ -160,10 +154,12 @@ def loop(self):
     def _gaze_direction(self, x):
         gaze_target_gate = min(len(self.gate_names) - 1, self.target_gate+1)
         pointing_direction = self.gate_mean[self.gate_names[gaze_target_gate]] - x
+        gaze_waypoint = 2*(self.target_gate+1)
+        pointing_direction = self.reference_waypoints[gaze_waypoint] - x
         if self.use_ir_markers:
             ir_ray = self.ir_waypoint()
-            if ir_ray is not None:
-                pointing_direction = ir_ray
+            #if ir_ray is not None and self.visible_target_markers >= 3:
+                #pointing_direction = ir_ray
         return pointing_direction
 
     def _orientation_quaternion(self, x):
@@ -174,34 +170,14 @@ def _orientation_quaternion(self, x):
         return math_utils.shortest_arc(np.array([1., 0., 0.]), gaze_direction)
 
     def _set_target(self, target_vector, state_estimate):
-        x, xdot, xddot, q = state_estimate
-
-        current_state = State()
-        current_state.header.stamp = rospy.Time.now()
-        current_state.header.frame_id = 'state_estimate'
-        current_state.pose.position = Vector3(x[0], x[1], x[2])
-        current_state.pose.orientation = Quaternion(q.x, q.y, q.z, q.w)
-        current_state.linear_velocity = Vector3(xdot[0], xdot[1], xdot[2])
-        current_state.linear_acceleration = Vector3(xddot[0], xddot[1], xddot[2])
-        self.state_pub.publish(current_state)
-
+        x, _, _, _ = state_estimate
         target_pose = Pose()
         target_pose.position = Vector3(target_vector[0], target_vector[1], target_vector[2])
-
-        q = self._orientation_quaternion(x)
+        # Using GT for the gaze direction, since we still haven't solved state estimation
+        q = self._orientation_quaternion(x)        
         target_pose.orientation = Quaternion(q.x, q.y, q.z, q.w)
         self.target_pub.publish(target_pose)
 
-    def reference_target_vector(self, x):
-        closest_dist = 1e9
-        closest_i = 0
-        for i in range(self.path.shape[0])[1:]:
-            new_d = np.linalg.norm(self.path[i, :] - x)
-            if new_d < closest_dist:
-                closest_dist = new_d
-                closest_i = i
-        return self.tangent_unit_vector(self.path[closest_i, :], self.path[closest_i+self.reference_lead_steps, :])
-
     def ir_waypoint(self):
         center = np.float32([0, 0])
         target_gate_name = None
@@ -227,25 +203,14 @@ def ir_waypoint(self):
                 gate_visible_area += 0.5 * np.linalg.norm(np.cross(r2 - r4, r3 - r4))
             self.latest_gate_visible_area = gate_visible_area
             # Unit vector in the direction of the average of IR markers in pixel space
-            return np.float32(camera_ray(mean_pixel[0], mean_pixel[1], q_b, corr=True))
+            return np.float32(camera_ray(mean_pixel[0:2], q_b, corr=True))
 
     def _ir_callback(self, msg):
         self.latest_markers = defaultdict(dict)
         self.latest_markers_time = msg.header.stamp
         for marker in msg.markers:
             self.latest_markers[marker.landmarkID.data][marker.markerID.data] = np.array([marker.x, marker.y])
 
-    def _imu_callback(self, msg):
-        omega = np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z])
-        omegabar = (omega + self.imu_latest_omega) / 2
-        xddot = np.array([msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z])
-        xdot = self.imu_latest_xdot + (xddot + self.imu_latest_xddot) / 2 * self.sample_time  # Integrate from xddot
-        x = self.imu_latest_x + (xdot + self.imu_latest_xdot) / 2 * self.sample_time  # Integrate from xdot
-        self.imu_latest_x = x
-        self.imu_latest_xdot = xdot
-        self.imu_latest_xddot = xddot
-        self.imu_latest_omega = omega
-
     def state_estimate(self, timestamp=None):
         if self.use_gt_state:
             use_timestamp = rospy.Time.now()
@@ -261,13 +226,15 @@ def state_estimate(self, timestamp=None):
                 xddot = self.tf_prev_xddot
                 if timestamp is None:
                     time_elapsed = rospy.Time.now() - self.prev_tf_time
-                    true_rate = 1/time_elapsed.to_sec()
-                    xdot = true_rate * (x - self.tf_prev_x)
-                    xddot = true_rate * (xdot - self.tf_prev_xdot)
-                    self.prev_tf_time = self.prev_tf_time + time_elapsed
-                    self.tf_prev_x = x
-                    self.tf_prev_xdot = xdot
-                    self.tf_prev_xddot = xddot
+                    secs = time_elapsed.to_sec()
+                    if secs > 0.0:
+                        true_rate = 1/secs
+                        xdot = true_rate * (x - self.tf_prev_x)
+                        xddot = true_rate * (xdot - self.tf_prev_xdot)
+                        self.prev_tf_time = self.prev_tf_time + time_elapsed
+                        self.tf_prev_x = x
+                        self.tf_prev_xdot = xdot
+                        self.tf_prev_xddot = xddot
                 return x, xdot, xddot, q
             except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                 return self.init_position, np.float32([0, 0, 0]), np.float32([0, 0, 0]), self.init_orientation

scripts/data_generator.py

@@ -13,7 +13,7 @@
 from mav_msgs.msg import RateThrust
 from sensor_msgs.msg import Imu
 from flightgoggles.msg import IRMarkerArray, IRMarker
-from lib.graphix import camera_ray
+from lib.vision import camera_ray
 from sensor_msgs.msg import Image