WIP #10: Lots of minor bug fixes - launch file for ekf test runs okay

slam/launch/test_ekf.launch

@@ -0,0 +1,29 @@
+<launch>
+
+    <!-- Parameters to define true pose for reference in error calculations-->
+    <param name="true_x" type="double" value="-1"/>
+    <param name="true_y" type="double" value="-1"/>
+    <param name="true_z" type="double" value="-1"/>
+    <param name="true_orientation_x" type="double" value="-1"/>
+    <param name="true_orientation_y" type="double" value="-1"/>
+    <param name="true_orientation_z" type="double" value="-1"/>
+    <param name="true_orientation_w" type="double" value="-1"/>
+    <param name="total_data_points" type="int" value="20"/>
+
+    <!-- parameters for image_publisher node -->
+    <param name="device_id" value="0"/>
+    <param name="calibration_file" value="camera_a.yaml"/>
+    <param name="publish_rate" value="20"/>
+    <param name="image_width" value="640"/>
+    <param name="image_height" value="480"/>
+
+    <!-- Run all the nodes required for the ekf test -->
+    <node name="image_publisher" pkg="vision" type="image_publisher.py"/>
+    <node name="aruco" pkg="vision" type="aruco_localization.py"/>
+    <node name="aruco_ekf" pkg="slam" type="ArucoEKF.py"/>
+    <node name="test_ekf" pkg="slam" type="testEKF.py"/>
+
+    <!-- Launch the rqt_console to view the log info printed at the end of the test -->
+    <node name="rqt_console" pkg="rqt_console" type="rqt_console"/>
+
+</launch>

slam/ArucoEKF.py → slam/scripts/ArucoEKF.py

@@ -29,7 +29,7 @@ def __init__(self):
         # Second arugment is dimension of the measurement input Z
         # For AruCo, measurement input is (x,y,theta)
 
-        self.arucoEKF = EKF.ExtendedKalmanFilter(6,6)
+        self.arucoEKF = EKF.ExtendedKalmanFilter(7,7)
 
         '''
         EKF.x initialized to zeros by constructor
@@ -38,7 +38,7 @@ def __init__(self):
         '''
 
         # Define the measurement noise covariance matrix
-        self.arucoEKF.R = np.eye(6) * 1
+        self.arucoEKF.R = np.eye(7) * 1
 
         # Define the process noise covariance matrix
         #arucoEKF.Q = 
@@ -48,16 +48,16 @@ def __init__(self):
 
     def update(self,pose):
         #Initialize empty np array
-        z = np.empty((6,1))
+        z = np.empty((7,1))
 
         #Convert Pose object into a 7x1 numpy array to pass to 
-        z[0][1] = pose.position.x
-        z[1][1] = pose.position.y
-        z[2][1] = pose.position.z
-        z[3][1] = pose.orientation.x
-        z[4][1] = pose.orientation.y
-        z[5][1] = pose.orientation.z
-        # z[6][1] = pose.orientation.w
+        z[0][0] = pose.position.x
+        z[1][0] = pose.position.y
+        z[2][0] = pose.position.z
+        z[3][0] = pose.orientation.x
+        z[4][0] = pose.orientation.y
+        z[5][0] = pose.orientation.z
+        z[6][0] = pose.orientation.w
 
         self.arucoEKF.predict_update(z, self.HJacobian, self.hx)
 
@@ -69,7 +69,7 @@ def getPose(self):
         pose.orientation.x = self.arucoEKF.x[3][0]
         pose.orientation.y = self.arucoEKF.x[4][0]
         pose.orientation.z = self.arucoEKF.x[5][0]
-        # pose.orientation.w = self.arucoEKF.x[0][6]
+        pose.orientation.w = self.arucoEKF.x[6][0]
 
         return Pose()
 

slam/testEKF.py → slam/scripts/testEKF.py

@@ -33,7 +33,6 @@ def __init__(self,trueX,trueY,trueZ,
         # Store raw and filtered aruco pose error
         self.arucoRaw = np.zeros((7, self.totalDataPoints))
         self.arucoEKF = np.zeros((7, self.totalDataPoints))
-        self.arucoEKFCov = np.zeros((6,6))
 
         # Store mean and standard deviation of all dimensions. 
         # The mean is stored in column 1 and the standard deviation in column 2.
@@ -59,22 +58,19 @@ def exportRaw(self,pose):
             self.indexRaw += 1
 
     # Record ekf aruco error
-    def exportFiltered(self,poseWithCovariance):
+    def exportFiltered(self,pose):
 
         # Collect up to specified number of data points
         if self.indexEKF < self.totalDataPoints:
 
             # Store ekf pose error in the next open array index
-            self.arucoEKF[0][self.indexEKF] = poseWithCovariance.pose.pose.x - self.trueX
-            self.arucoEKF[1][self.indexEKF] = poseWithCovariance.pose.pose.y - self.trueY
-            self.arucoEKF[2][self.indexEKF] = poseWithCovariance.pose.pose.z - self.trueZ
-            self.arucoEKF[3][self.indexEKF] = poseWithCovariance.pose.orientation.x - self.trueOrientationX
-            self.arucoEKF[4][self.indexEKF] = poseWithCovariance.pose.orientation.y - self.trueOrientationY
-            self.arucoEKF[5][self.indexEKF] = poseWithCovariance.pose.orientation.z - self.trueOrientationZ
-            self.arucoEKF[6][self.indexEKF] = poseWithCovariance.pose.orientation.w - self.trueOrientationW
-
-            # Store ekf covariance data in the next open array index
-            self.arucoEKFCov = poseWithCovariance.pose.covariance
+            self.arucoEKF[0][self.indexEKF] = pose.position.x - self.trueX
+            self.arucoEKF[1][self.indexEKF] = pose.position.y - self.trueY
+            self.arucoEKF[2][self.indexEKF] = pose.position.z - self.trueZ
+            self.arucoEKF[3][self.indexEKF] = pose.orientation.x - self.trueOrientationX
+            self.arucoEKF[4][self.indexEKF] = pose.orientation.y - self.trueOrientationY
+            self.arucoEKF[5][self.indexEKF] = pose.orientation.z - self.trueOrientationZ
+            self.arucoEKF[6][self.indexEKF] = pose.orientation.w - self.trueOrientationW
 
             # Increment EKF index
             self.indexEKF += 1
@@ -89,8 +85,7 @@ def calculateMeanAndStd(self):
             self.arucoEKFMeanAndStd[x][0] = np.mean(self.arucoEKF[x])
             self.arucoEKFMeanAndStd[x][1] = np.std(self.arucoEKF[x])
 
-        print(self.arucoRawMeanAndStd)
-        print(self.arucoEKFMeanAndStd)
+        return self.arucoRawMeanAndStd, self.arucoEKFMeanAndStd
 
 
 # Run ROS Node
@@ -114,14 +109,15 @@ def calculateMeanAndStd(self):
                         trueOrientationX,trueOrientationY,trueOrientationZ,trueOrientationW,totalDataPoints)
 
     # Create subscribers to raw and filtered pose topics
-    poseSubscriber_raw = rospy.Subscriber('aruco_pose_raw', Pose, aruco_pose_tracker.exportRaw)
-    poseSubscriber_filter_cov = rospy.Subscriber('aruco_pose_filtered_covariance', PoseWithCovarianceStamped,
-                                                 aruco_pose_tracker.exportFiltered)
+    poseSubscriber_raw = rospy.Subscriber('aruco/pose_raw', Pose, aruco_pose_tracker.exportRaw)
+    poseSubscriber_filter_cov = rospy.Subscriber('ekf/pose_filtered', Pose, aruco_pose_tracker.exportFiltered)
 
     # Wait until enough data points have been collected
-    while aruco_pose_tracker.indexRaw <= totalDataPoints and \
-            aruco_pose_tracker.indexEKF <= totalDataPoints:
+    while aruco_pose_tracker.indexRaw < totalDataPoints and \
+            aruco_pose_tracker.indexEKF < totalDataPoints and not rospy.is_shutdown():
         pass
 
     # Calculate mean and standard deviation once done collecting data. Print to console.
-    aruco_pose_tracker.calculateMeanAndStd()
+    raw_statistics, filtered_statistics = aruco_pose_tracker.calculateMeanAndStd()
+    rospy.loginfo("\nStatistics for raw pose data:\n\n" + str(raw_statistics))
+    rospy.loginfo("\nStatistics for filtered data:\n\n" + str(filtered_statistics))

vision/scripts/image_publisher.py

@@ -1,4 +1,4 @@
-
+#!/usr/bin/env python
 
 # Import ROS packages
 import rospy
@@ -9,17 +9,18 @@
 import cv2
 import yaml
 import numpy as np
+import sys
 
 # Configuration params
 DEFAULT_DEVICE_ID = 0
 DEFAULT_CALIB_FILE = 'camera_a.yaml'
-DEFAULT_PUB_RATE = 1
+DEFAULT_PUB_RATE = 20
 DEFAULT_IMG_WIDTH = 640
 DEFAULT_IMG_HEIGHT = 480
 DEFAULT_ALPHA = 1
 
 # Other params
-CALIBRATION_FILE_DIR = "camera_calibration/calibration_data/"
+CALIBRATION_FILE_DIR = sys.path[0] + "/camera_calibration/calibration_data/"
 
 # Run the node
 if __name__ == '__main__':
@@ -53,7 +54,7 @@
     bridge = CvBridge()
 
     # Loop indefinitely
-    rate = rospy.Rate(29)
+    rate = rospy.Rate(publish_rate)
     while not rospy.is_shutdown():
 
         # Get next frame

vision/scripts/marker_detection/aruco_localization.py

@@ -10,13 +10,14 @@
 
 # Import other required packages
 import cv2.aruco as aruco
-import cv2
 import numpy as np
 import yaml
+import sys
 
 
 DEFAULT_CALIB_FILE = 'camera_a.yaml'
-CALIBRATION_FILE_DIR = '../camera_calibration/calibration_data/'
+CALIBRATION_FILE_DIR = sys.path[0] + '/../camera_calibration/calibration_data/'
+
 
 class ImageHandler: