Merge branch 'main' of https://git.soton.ac.uk/plw1g21/robobin

ros2/src/robobin/robobin/uwb_navigation_node.py

@@ -122,6 +122,10 @@ class UWBNode(Node):
         self.anchors_coords_known = False
         self.previous_tag1_position = None
 
+        self.windowSize = 10
+        self.tag1Window = []
+        self.tag2Window = []
+
         self.current_tag1_position = None
         self.current_tag2_position = None
 
@@ -304,10 +308,10 @@ class UWBNode(Node):
         bounds = ([x_min, y_min, z_min], [x_max, y_max, z_max])
 
         def error_function(vars):
-            x, y = vars
+            x, y, z = vars
             residuals = []
-            for (bx, by), d_measured in zip(available_beacons, available_distances):
-                d_computed = np.sqrt((x - bx)**2 + (y - by)**2)
+            for (bx, by, bz), d_measured in zip(available_beacons, available_distances):
+                d_computed = np.sqrt((x - bx)**2 + (y - by)**2 + (z - bz)**2)
                 residuals.append(d_computed - d_measured)
             return residuals
 
@@ -332,13 +336,16 @@ class UWBNode(Node):
 
                 tag1_position = self.calculate_tag_coordinates(tag_distances_dict)
 
-                
-
+                if tag1_position:
+                    if len(self.tag1Window < self.windowSize):
+                        self.tag1Window.append(tag1_position)
+                    else:
+                        self.tag1Window = self.tag1Window[1:] + [tag1_position]
+                    
+                    self.current_tag1_position = tuple(map(lambda li: sum(li) / len(li), zip(self.tag1Window)))
 
-                if tag1_position is not None:
-                    self.current_tag1_position = tag1_position
                     self.get_logger().info(f"Tag 1 position: {tag1_position}")
-                    position_msg = Point(x=tag1_position[0], y=tag1_position[1], z=tag1_position[2])
+                    position_msg = Point(x=self.current_tag1_position[0], y=self.current_tag1_position[1], z=self.current_tag1_position[2])
                     self.publisher.publish(position_msg)
 
                 if self.mode == "Follow":
@@ -353,22 +360,29 @@ class UWBNode(Node):
                         }
                         tag2_position = self.calculate_tag_coordinates(tag2_distances_dict)
                         if tag2_position:
-                            self.get_logger().info(f"Tag 1 position: {tag1_position}")
-                            self.get_logger().info(f"Tag 2 position: {tag2_position}")
-                            self.current_tag2_position = tag2_position
-                            target_msg = Point(x=tag2_position[0], y=tag2_position[1], z=tag2_position[2])
+                            if len(self.tag2Window < self.windowSize):
+                                self.tag2Window.append(tag2_position)
+                            else:
+                                self.tag2Window = self.tag2Window[1:] + [tag2_position]
+                            
+                            self.current_tag2_position = tuple(map(lambda li: sum(li) / len(li), zip(self.tag2Window)))
+
+                            self.get_logger().info(f"Tag 1 position: {self.current_tag1_position}")
+                            self.get_logger().info(f"Tag 2 position: {self.current_tag2_position}")
+                            target_msg = Point(x=self.current_tag2_position[0], y=self.current_tag2_position[1], z=self.current_tag2_position[2])
                             self.target_location_pub.publish(target_msg)
 
                             # Compute offset if not yet computed
                             if not self.follow_offset_computed and self.current_yaw is not None and self.current_tag1_position is not None:
                                 self.uwb_to_imu_offset = self.compute_heading_offset(self.current_tag1_position, self.current_tag2_position, self.current_yaw)
                                 self.follow_offset_computed = True
-
+                        else:
+                            self.tag2Window = []
                     else:
                         self.get_logger().warning("Tag 2 not known")
+                        self.tag2Window = []
                 
                 if self.mode == "Call" and self.call_mode_active:
-              
                     self.get_logger().info("Call mode active.")
                     if self.current_tag1_position is not None and self.current_target_index < len(self.path):
                         self.get_logger().info(f"Current target index: {self.current_target_index}")
@@ -387,7 +401,7 @@ class UWBNode(Node):
                         # Check if close to the node
                         dist = math.sqrt((self.current_tag1_position[0] - target_pos[0])**2 + 
                                         (self.current_tag1_position[1] - target_pos[1])**2)
-                        if dist < 5:  # Threshold of 5 cm
+                        if dist < 10:  # Threshold of 5 cm
                             self.current_target_index += 1
                             if self.current_target_index >= len(self.path):
                                 self.get_logger().info("Call mode: Reached final destination.")
@@ -395,8 +409,6 @@ class UWBNode(Node):
                             else:
                                 self.get_logger().info(f"Moving to next node: {self.path[self.current_target_index]}")
 
-
-
             except Exception as e:
                 self.get_logger().error(f"Error updating positions: {e}")
 #ros2\src\robobin\robobin\graphs\graph.json