WIP #10: Fixed a bug in EKF code, a few slight changes to test script

slam/launch/test_ekf.launch

@@ -1,14 +1,14 @@
 <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"/>
+    <param name="true_x" type="double" value="0"/>
+    <param name="true_y" type="double" value="0"/>
+    <param name="true_z" type="double" value="1"/>
+    <param name="true_orientation_x" type="double" value="0"/>
+    <param name="true_orientation_y" type="double" value="0"/>
+    <param name="true_orientation_z" type="double" value="0"/>
+    <param name="true_orientation_w" type="double" value="1"/>
+    <param name="total_data_points" type="int" value="500"/>
 
     <!-- parameters for image_publisher node -->
     <param name="device_id" value="0"/>

slam/scripts/ArucoEKF.py

@@ -71,7 +71,7 @@ def getPose(self):
         pose.orientation.z = self.arucoEKF.x[5][0]
         pose.orientation.w = self.arucoEKF.x[6][0]
 
-        return Pose()
+        return pose
 
     def getPoseCovStamped(self):
         poseCovStamped = PoseWithCovarianceStamped()

slam/scripts/testEKF.py

@@ -6,6 +6,7 @@
 # import ros packages
 import rospy
 from geometry_msgs.msg import Pose, PoseWithCovarianceStamped
+from std_msgs.msg import Bool
 
 # Import other required packages
 import numpy as np
@@ -26,6 +27,9 @@ def __init__(self,trueX,trueY,trueZ,
         self.trueOrientationW = trueOrientationW
         self.totalDataPoints = totalDataPoints
 
+        # Only store data if board is detected
+        self.isBoardDetected = False
+
         # Indices to track how many data points we have read in out of the total
         self.indexRaw = 0
         self.indexEKF = 0
@@ -39,11 +43,15 @@ def __init__(self,trueX,trueY,trueZ,
         self.arucoRawMeanAndStd = np.zeros((7,2))
         self.arucoEKFMeanAndStd = np.zeros((7,2))
 
+    # Update whether marker is detected or not
+    def updateMarkerDetected(self, msg):
+        self.isBoardDetected = msg.data
+
     # Record raw aruco error
     def exportRaw(self,pose):
 
         # Collect up to specified number of data points
-        if self.indexRaw < self.totalDataPoints:
+        if self.indexRaw < self.totalDataPoints and self.isBoardDetected:
 
             # Store raw error in the next open array index
             self.arucoRaw[0][self.indexRaw] = pose.position.x - self.trueX
@@ -61,7 +69,7 @@ def exportRaw(self,pose):
     def exportFiltered(self,pose):
 
         # Collect up to specified number of data points
-        if self.indexEKF < self.totalDataPoints:
+        if self.indexEKF < self.totalDataPoints and self.isBoardDetected:
 
             # Store ekf pose error in the next open array index
             self.arucoEKF[0][self.indexEKF] = pose.position.x - self.trueX
@@ -78,6 +86,8 @@ def exportFiltered(self,pose):
     # Calculate mean and standard deviation 
     def calculateMeanAndStd(self):
 
+        rospy.loginfo(self.arucoRaw[2])
+
         # Calculates the raw and ekf mean and standard deviation in all dimensions
         for x in range(7):
             self.arucoRawMeanAndStd[x][0] = np.mean(self.arucoRaw[x])
@@ -111,6 +121,7 @@ def calculateMeanAndStd(self):
     # 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('ekf/pose_filtered', Pose, aruco_pose_tracker.exportFiltered)
+    markerDetectedSub = rospy.Subscriber('aruco/marker_detected', Bool, aruco_pose_tracker.updateMarkerDetected)
 
     # Wait until enough data points have been collected
     while aruco_pose_tracker.indexRaw < totalDataPoints and \