A few changes, don't really know tbh, nothing important

robot_navigation/scripts/pure_pursuit.py

@@ -1,4 +1,4 @@
-
+#!/usr/bin/env python
 
 # Import ROS packages
 import rospy
@@ -132,8 +132,8 @@ def compute_new_twist(self, new_pose):
     rospy.init_node("pure_pursuit")
 
     # Read parameters off server
-    lin_vel = rospy.get_param("pp_base_lin_vel", default=0.5)
-    max_lookahead = rospy.get_param("pp_max_lookahead", default=0.05)
+    lin_vel = rospy.get_param("pp_base_lin_vel", default=0.1)
+    max_lookahead = rospy.get_param("pp_max_lookahead", default=1.0)
 
     # Create pure pursuit object
     pp = PurePursuit(lin_vel, max_lookahead)

robot_simulation/scripts/field_visualization.py

@@ -1,8 +1,8 @@
 #!/usr/bin/env python
 
-# ----------------------------- #
-# Subscribes to drive command, Publishes custom motor speeds. #
-# ----------------------------- #
+# ------------------------------------------------------- #
+# Publishes markers for visualizing the competition arena #
+# ------------------------------------------------------- #
 
 # Import ROS packages
 import rospy
@@ -75,28 +75,28 @@ def __init__(self, frame_id, pose_array, x_len, y_len, z_len, color_array):
     # -- Add walls -- #
 
     # Front
-    wall_front_pose = [-wall_width, -wall_width, 0, 0, 0, 0, 0]
+    wall_front_pose = [-wall_width / 2, -wall_width, 0, 0, 0, 0, 0]
     wall_front_msg = RectangleMarker("world", wall_front_pose, field_width + (wall_width * 2),
                                      wall_width, wall_height, wall_color)
 
     # Back
-    wall_back_pose = [-wall_width, field_length, 0, 0, 0, 0, 0]
+    wall_back_pose = [-wall_width / 2, field_length, 0, 0, 0, 0, 0]
     wall_back_msg = RectangleMarker("world", wall_back_pose, field_width + (wall_width * 2),
                                     wall_width, wall_height, wall_color)
 
     # Left
-    wall_left_pose = [-wall_width, 0, 0, 0, 0, 0, 0]
+    wall_left_pose = [-wall_width / 2, 0, 0, 0, 0, 0, 0]
     wall_left_msg = RectangleMarker("world", wall_left_pose, wall_width,
                                     field_length, wall_height, wall_color)
 
     # Right
-    wall_right_pose = [field_width, 0, 0, 0, 0, 0, 0]
+    wall_right_pose = [field_width + (wall_width / 2), 0, 0, 0, 0, 0, 0]
     wall_right_msg = RectangleMarker("world", wall_right_pose, wall_width,
                                      field_length, wall_height, wall_color)
 
     # -- Add ground -- #
 
-    ground_pose = [-wall_width, -wall_width, 0, 0, 0, 0, 0]
+    ground_pose = [-wall_width / 2, -wall_width, 0, 0, 0, 0, 0]
     ground_msg = RectangleMarker("world", ground_pose, field_width + (wall_width * 2), field_length + (wall_width * 2), 0.001, ground_color)
 
     # Add all the messages to the marker array

robot_simulation/urdf/robot_model.urdf

@@ -0,0 +1,17 @@
+<robot name="test_robot">
+
+  <material name="blue">
+    <color rgba="0 0 0.5 1"/>
+  </material>
+
+  <link name="robot_frame">
+    <visual>
+
+      <geometry>
+        <mesh filename="package://robot_simulation/cad/full_robot.stl"  scale="0.01 0.01 0.01"/>
+      </geometry>
+      <origin rpy="1.57075 0.0 0.0" xyz="0.0 0.0 0.5"/>
+      <material name="blue"/>
+    </visual>
+  </link>
+</robot>

robot_slam/ekf/ArucoEKF.py

@@ -63,13 +63,13 @@ def update(self,pose):
 
     def getPose(self):
         pose = Pose()
-        pose.position.x = self.arucoEKF.x[0][0]
-        pose.position.y = self.arucoEKF.x[1][0]
+        pose.position.x = 0.0 #self.arucoEKF.x[0][0]
+        pose.position.y = 0.0 #self.arucoEKF.x[1][0]
         pose.position.z = self.arucoEKF.x[2][0]
         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[6][0]
+        pose.orientation.w = 1.0 #self.arucoEKF.x[6][0]
 
         return pose
 
@@ -82,9 +82,9 @@ def getPoseCovStamped(self):
         self.seqID += 1 
 
         poseCovStamped.pose.pose = self.getPose()
-        poseCovStamped.pose.covariance = self.arucoEKF.P
+        poseCovStamped.pose.covariance = np.reshape(np.eye(6)*5, (36,)).tolist()
 
-        return PoseWithCovarianceStamped()
+        return poseCovStamped
 
 
     # Define the Jacobian matrix for measurement update