feat: control system rewrite in preperation for expo

Author: prototypicalpro

CMakeLists.txt

@@ -57,14 +57,14 @@ catkin_python_setup()
 add_message_files(
   FILES
   PointWithCovarianceStamped.msg
-  RegionOfInterestWithCovarianceStamped.msg
+  RegionOfInterestWithConfidenceStamped.msg
 )
 
 ## Generate services in the 'srv' folder
 add_service_files(
    FILES
-   MoveitPose.srv
    Tf2Transform.srv
+   Tf2TransformPoseStamped.srv
 )
 
 ## Generate actions in the 'action' folder

bin/applevision_expo_demo.py

@@ -5,38 +5,31 @@
 import os
 import subprocess
 import sys
-import select
-from threading import Lock, Thread
+from threading import Lock
+import math
 
 import tf2_ros
 import moveit_commander
 import rospy
 from message_filters import Cache, Subscriber
 from moveit_msgs.msg import MoveGroupActionFeedback
-from sensor_msgs.msg import Range
 from visualization_msgs.msg import Marker
-from geometry_msgs.msg import TransformStamped
-from applevision_rospkg.srv import Tf2Transform
 
 
-class ProxyStdout(Thread):
-    def __init__(self, stdout, prefix, **kwargs):
-        super(ProxyStdout, self).__init__(**kwargs)
-        self.daemon = True
-        self.prefix = prefix
-        self.stdout = stdout
-
-    def run(self):
-        while not self.stdout.closed:
-            line = self.stdout.readline()
-            if not line:
-                break
-            rospy.loginfo(self.prefix + line.strip())
+def quaternion_multiply(quaternion1, quaternion0):
+    x0, y0, z0, w0 = quaternion0
+    x1, y1, z1, w1 = quaternion1
+    return ([-x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
+                     x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
+                     -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
+                     x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0])
 
 class ExpoDemo():
     BOUNDS_X = (-0.8, -0.2)
     BOUNDS_Y = (-1.2, 0)
     BOUNDS_Z = (1, 2)
+    BOUND_ORIENTATION = 20
+    ORIENTATION_TARGET = (-0.707, 0, 0, 0.707)
     EXPO_DEMO_PROGRAM = os.path.join(os.path.dirname(__file__), 'applevision_motion.py')
     CHECK_DURATION = rospy.Duration.from_sec(0.1)
     GROUP_NAME = 'manipulator'
@@ -85,9 +78,7 @@ def __init__(self):
         rospy.on_shutdown(self.kill_robot)
 
     def make_control_system_process(self):
-        proc = subprocess.Popen(
-            [self.EXPO_DEMO_PROGRAM], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, universal_newlines=True)
-        ProxyStdout(proc.stdout, 'EXPO: DEMO {}:'.format(self.demo_seq)).start()
+        proc = subprocess.Popen([self.EXPO_DEMO_PROGRAM, 'EXPO: DEMO {}:'.format(self.demo_seq)])
         self.demo_seq += 1
         return proc
 
@@ -137,6 +128,17 @@ def is_home(self):
     def demo_is_running(self):
         return self.demo_process is not None and self.demo_process.poll() is None
 
+    def is_position_out(self, coords):
+        return coords.x < ExpoDemo.BOUNDS_X[0] or coords.x > ExpoDemo.BOUNDS_X[1] \
+                or coords.y < ExpoDemo.BOUNDS_Y[0] or coords.y > ExpoDemo.BOUNDS_Y[1] \
+                or coords.z < ExpoDemo.BOUNDS_Z[0] or coords.z > ExpoDemo.BOUNDS_Z[1]
+    
+    def is_orientation_out(self, quat):
+        # find the smallest angle between this quaternion and the target, and check it's within range
+        new = quaternion_multiply((-quat.x, -quat.y, -quat.z, quat.w), self.ORIENTATION_TARGET)
+        angle = abs(180.0 - math.degrees(2*math.atan2(math.sqrt(new[0]**2 + new[1]**2 + new[2]**2), new[3])))
+        return angle > self.BOUND_ORIENTATION
+
     def check_cb(self, *args):
         if self.running_lock.locked():
             return
@@ -148,10 +150,10 @@ def check_cb(self, *args):
             except Exception as e:
                 self.die_gracefully('EXPO: tf failed with error {}. Probably time to restart the system.'.format(e))
             coords = where_in_world.transform.translation
-            is_out_bounds = coords.x < ExpoDemo.BOUNDS_X[0] or coords.x > ExpoDemo.BOUNDS_X[1] \
-                or coords.y < ExpoDemo.BOUNDS_Y[0] or coords.y > ExpoDemo.BOUNDS_Y[1] \
-                or coords.z < ExpoDemo.BOUNDS_Z[0] or coords.z > ExpoDemo.BOUNDS_Z[1]
-            
+            is_out_bounds = self.is_position_out(coords)
+            orientation = where_in_world.transform.rotation
+            is_out_orientation = self.is_orientation_out(orientation)
+
             # count idle ticks
             if self.demo_is_running() and not self.is_moving():
                 self.idle_count += 1
@@ -175,6 +177,10 @@ def check_cb(self, *args):
                     coord_str = str(coords).replace('\n', ' ')
                     rospy.logwarn('EXPO: bounds check failed with coordinates {}, terminating and reseting...'.format(coord_str))
                     self.kill_robot()
+                elif is_out_orientation:
+                    # the robot is doing disco moves
+                    rospy.logwarn('EXPO: bounds check failed for orientation, terminating and reseting...')
+                    self.kill_robot()
                 elif self.idle_count > self.TICK_TIMEOUT:
                     # the robot has been idle (not controlled by this program) for too long
                     rospy.logwarn('EXPO: demo got stuck, restarting...')
@@ -192,7 +198,6 @@ def check_cb(self, *args):
                         self.die_gracefully('EXPO: demo crashed on launch, system cannot recover.')
                 else:
                     # the demo is crashed and the robot is stuck in another position
-                    rospy.sleep(1)
                     rospy.loginfo('EXPO: Sending robot home...')
                     self.go_home() 
 

bin/applevision_filter.py

@@ -1,23 +1,24 @@
 #!/usr/bin/python3
 
+from typing import Optional
 import rospy
 import numpy as np
 from tf2_msgs.msg import TFMessage
 from geometry_msgs.msg import TransformStamped, PoseWithCovarianceStamped
 from sensor_msgs.msg import Range
-from message_filters import ApproximateTimeSynchronizer, Subscriber
-from applevision_rospkg.msg import PointWithCovarianceStamped, RegionOfInterestWithCovarianceStamped
+from message_filters import Subscriber
+from applevision_rospkg.msg import PointWithCovarianceStamped, RegionOfInterestWithConfidenceStamped
 from applevision_rospkg.srv import Tf2Transform
 from applevision_kalman.filter import KalmanFilter, EnvProperties
-from helpers import RobustServiceProxy, ServiceProxyFailed, HeaderCalc, CameraInfoHelper
+from helpers import RobustServiceProxy, ServiceProxyFailed, HeaderCalc, CameraInfoHelper, SynchronizerMinTick
 
 
 def second_order_var_approx(fprime, fdoubleprime, ftrippleprime, xvar):
     # https://en.wikipedia.org/wiki/Taylor_expansions_for_the_moments_of_functions_of_random_variables
     return fprime**2 * xvar + 1 / 2 * fdoubleprime**2 * xvar**2 + fprime * ftrippleprime * xvar**2
 
 
-class MainHandler:
+class FilterHandler:
 
     def __init__(self, topic: str, kal: KalmanFilter) -> None:
         self.tf_get = RobustServiceProxy('Tf2Transform',
@@ -35,13 +36,8 @@ def __init__(self, topic: str, kal: KalmanFilter) -> None:
         self._header = HeaderCalc('palm')
         self._gen = np.random.default_rng()
 
-    def callback(self, dist: Range,
-                 cam: RegionOfInterestWithCovarianceStamped):
-        self.cam_info.wait_for_camera_info()
-        cam_info_obj = self.cam_info.get_last_camera_info()
-        cam_res = np.array([cam_info_obj.width, cam_info_obj.height])
-        cam_focal = np.array([cam_info_obj.P[0], cam_info_obj.P[5]])
-
+    def callback(self, dist: Optional[Range],
+                 cam: Optional[RegionOfInterestWithConfidenceStamped]):
         try:
             dist_to_home = self.tf_get('palm', 'applevision_start_pos',
                                        rospy.Time(), rospy.Duration())
@@ -54,46 +50,8 @@ def callback(self, dist: Range,
                 trans.transform.translation.x, trans.transform.translation.y,
                 trans.transform.translation.z
             ]))
-        range_corrected = dist.range + self.kal.env.apple_r
-
-        # publish the kalman filters predicted apple distance
-        # if bounding box is on the edge the variance is infinite
-        if cam.w == 0 or cam.h == 0 or cam.x == 0 or cam.x + cam.w == cam_res[
-                0]:
-            x_est, p_est, var = self.kal.step_filter(
-                (0, 0, 0, range_corrected), np.inf, np.inf, np.inf, control)
-        else:
-            # compute apple x, y based off of the bounding box width/height avg
-            # TODO: how to fix this? it's unreliable
-            # TODO: improve varience calculations
-            # TODO: initialize robot in correct position
-            # TODO: this is wrong
-            est_z = cam_focal[0] * (2 * self.kal.env.apple_r) / cam.w
-            center_x = cam.x + cam.w / 2 - cam_res[0] / 2
-            center_y = cam.y + cam.h / 2 - cam_res[1] / 2
-            est_x = center_x * est_z / cam_focal[0]
-            est_y = center_y * est_z / cam_focal[1]
-
-            z_const = cam_focal[0] * (2 * self.kal.env.apple_r)
-            z_fprime = z_const * -1 / cam.w**2
-            z_fdoubleprime = z_const * 2 / cam.w**3
-            z_ftrippleprime = z_const * -6 / cam.w**4
-            z_var = second_order_var_approx(z_fprime, z_fdoubleprime,
-                                            z_ftrippleprime, cam.w_var)
-
-            # assume x and w uncorrelated (probably not true)
-            center_x_var = cam.x_var + 1 / 4 * cam.w_var
-            center_y_var = cam.x_var + 1 / 4 * cam.w_var
-            # https://stats.stackexchange.com/questions/52646/variance-of-product-of-multiple-independent-random-variables
-            x_var = ((center_x_var + center_x**2) * (z_var + est_z**2) -
-                     (center_x * est_z)**2) * 1 / cam_focal[0]**2
-            y_var = ((center_y_var + center_y**2) * (z_var + est_z**2) -
-                     (center_y * est_z)**2) * 1 / cam_focal[0]**2
-
-            # TODO: kalman filter will runaway sometimes?
-            x_est, p_est, var = self.kal.step_filter(
-                (est_x, est_y, est_z, range_corrected), x_var, y_var, z_var,
-                control)
+
+        x_est, p_est, var = self.kal.step_filter(cam, dist, control)
 
         trans_out = TransformStamped()
         trans_out.header = self._header.get_header()
@@ -107,8 +65,8 @@ def callback(self, dist: Range,
 
         out = PointWithCovarianceStamped()
         out.header = self._header.get_header()
-        out.camera_stamp = cam.header.stamp
-        out.distance_stamp = dist.header.stamp
+        out.camera_stamp = cam.header.stamp if cam else rospy.Time()
+        out.distance_stamp = dist.header.stamp if dist else rospy.Time()
         out.point = x_est.tolist()
         out.covariance = p_est.flatten().tolist()
         self.p_out.publish(out)
@@ -126,6 +84,11 @@ def main():
     rospy.init_node('applevision_filter')
     rospy.wait_for_service('Tf2Transform')
 
+    rospy.loginfo('Waiting for camera info...')
+    cam_info = CameraInfoHelper('palm_camera/camera_info')
+    cam_info.wait_for_camera_info()
+    rospy.loginfo('Got it! Starting...')
+
     # TODO: Tune these
     env = EnvProperties(delta_t_ms=33,
                         accel_std=0.005,
@@ -134,17 +97,24 @@ def main():
                         z_std=.005,
                         backdrop_dist=.5,
                         apple_r=.040,
-                        dist_fov_rad=np.deg2rad(20))
-    kal_filter = KalmanFilter(env, 1.5, 0.3, 0.75, 0.9)
+                        dist_fov_rad=np.deg2rad(20),
+                        camera_info=cam_info.get_last_camera_info())
+    kal_filter = KalmanFilter(
+        env,
+        std_range=1.5,
+        too_close_cam=0.3,
+        too_far_dist=7,
+        appl_proportion_low=0.75,
+        appl_proportion_high=0.9)
     rospy.logdebug(
         f'Kalman filter using environment {env} and filter {kal_filter}.')
 
     # input('Press any key to start Applevision...')
-    main_proc = MainHandler('applevision/est_apple_pos', kal_filter)
+    main_proc = FilterHandler('applevision/est_apple_pos', kal_filter)
     dist = Subscriber('applevision/apple_dist', Range)
     camera = Subscriber('applevision/apple_camera',
-                        RegionOfInterestWithCovarianceStamped)
-    sync = ApproximateTimeSynchronizer([dist, camera], 10, 0.017)
+                        RegionOfInterestWithConfidenceStamped)
+    sync = SynchronizerMinTick([dist, camera], queue_size=10, slop=0.017, min_tick=0.2)
     sync.registerCallback(main_proc.callback)
 
     rospy.spin()