took smoothing out of auto and added to teleop

all_seaing_controller/all_seaing_controller/xdrive_controller.py

@@ -56,7 +56,7 @@ def __init__(self):
             "smoothing_factor", 0.8).get_parameter_value().double_value
         self.curr_output = np.zeros(4)
         self.prev_output = np.zeros(4)
-        
+
         # From the T200 datasheet, approximately 40N maximum force
         THRUST_CONST = 40.0
         thrust_force_x = THRUST_CONST * math.sin(thruster_angle)
@@ -78,7 +78,7 @@ def __init__(self):
         self.front_left_pub = self.create_publisher(Float64, "thrusters/front_left/thrust", 10)
         self.back_right_pub = self.create_publisher(Float64, "thrusters/back_right/thrust", 10)
         self.timer = self.create_timer(TIMER_PERIOD, self.timer_cb)
-    
+
     def calculate_control_output(self, target_vel):
         target_vel_sq = np.sign(target_vel) * np.square(target_vel)
         constraint_bounds = self.drag_constants @ target_vel_sq
@@ -93,7 +93,7 @@ def calculate_control_output(self, target_vel):
         if not result.success:
             self.get_logger().error("Optimization failed to converge!")
             return None
-        
+
         control_output = result.x.reshape(4)
         if np.any(np.abs(control_output) > 1):
             control_output = control_output / np.max(np.abs(control_output))
@@ -120,9 +120,9 @@ def timer_cb(self):
         """
 
         # Smoothing via low-pass filter
-        control_output = self.smoothing * self.prev_output + (1 - self.smoothing) * self.curr_output
-        self.prev_output = control_output
-        
+        control_output = self.curr_output
+        # self.prev_output = control_output
+
         # Scale control_output from [-1,1] to output range
         scaled_control_output = control_output * (self.output_range[1] - self.output_range[0]) / 2
         thrust_cmd = scaled_control_output + np.mean(self.output_range)

all_seaing_driver/all_seaing_driver/onshore_node.py

@@ -19,10 +19,15 @@ def __init__(self):
         self.declare_parameter("joy_x_scale", 2.0)
         self.declare_parameter("joy_y_scale", -1.0)
         self.declare_parameter("joy_ang_scale", -0.8)
+        self.declare_parameter("smoothing_factor", 0.8)
 
         self.joy_x_scale = self.get_parameter("joy_x_scale").value
         self.joy_y_scale = self.get_parameter("joy_y_scale").value
         self.joy_ang_scale = self.get_parameter("joy_ang_scale").value
+        self.smoothing = self.get_parameter("smoothing_factor").value
+
+        self.prev_output_x = 0
+        self.prev_output_y = 0
 
         self.heartbeat_message = Heartbeat()
         self.heartbeat_publisher = self.create_publisher(Heartbeat, "heartbeat", 10)
@@ -47,8 +52,10 @@ def beat_heart(self):
     def send_controls(self, x, y, angular):
         control_option = ControlOption()
         control_option.priority = 0  # TeleOp has the highest priority value
-        control_option.twist.linear.x = x
-        control_option.twist.linear.y = y
+        control_option.twist.linear.x = self.smoothing * self.prev_output_x + (1 - self.smoothing) * x
+        self.prev_output_x = control_option.twist.linear.x
+        control_option.twist.linear.y = self.smoothing * self.prev_output_y + (1 - self.smoothing) * y
+        self.prev_output_y = control_option.twist.linear.y
         control_option.twist.angular.z = angular
         self.control_option_pub.publish(control_option)
 

all_seaing_driver/all_seaing_driver/rover_lora_combined.py

@@ -16,51 +16,60 @@ def __init__(self):
         # Setup serial connection
         self.serial_port = serial.Serial('/dev/ttyUSB0', 57600, timeout=1)
         time.sleep(2)  # Allow serial port to stabilize
-        
+
         self.data_size = struct.calcsize('BdddBBB')
 
+        self.smoothing = self.declare_parameter("smoothing_factor", 0.8).get_parameter_value().double_value
+        self.prev_output_x = 0
+        self.prev_output_y = 0
+
         # Create ROS 2 publisher for ControlOption
         self.control_publisher = self.create_publisher(ControlOption, 'control_options', 10)
 
         # Start a ROS 2 timer to check for serial data
         self.timer = self.create_timer(0.01, self.check_serial_data)
-                
+
         self.heartbeat_publisher = self.create_publisher(Heartbeat, "heartbeat", 10)
-        
-        
+
+
     def calculate_checksum(self, data):
         return sum(data) % 256
 
     def check_serial_data(self):
         if self.serial_port.in_waiting > 0:
             # Read serialized JSON data from the serial port
             serialized_data = self.serial_port.read(self.data_size)
-            
+
             if len(serialized_data) != self.data_size:
                 return
-        
+
             priority, x, y, angular, in_teleop, e_stopped, received_checksum = struct.unpack('BdddBBB', serialized_data)
-            
+
             # Verify checksum
             data_without_checksum = serialized_data[:-1]
             calculated_checksum = self.calculate_checksum(data_without_checksum)
-            
+
             if calculated_checksum != received_checksum:
                 print("Checksum mismatch, discarding data")
                 return
-                        
+
             control_msg = ControlOption()
             control_msg.priority = priority
-            control_msg.twist.linear.x = x
-            control_msg.twist.linear.y = y
+
+            control_msg.twist.linear.x = self.smoothing * self.prev_output_x + (1 - self.smoothing) * x
+            self.prev_output_x = control_msg.twist.linear.x
+
+            control_msg.twist.linear.y = self.smoothing * self.prev_output_y + (1 - self.smoothing) * y
+            self.prev_output_y = control_msg.twist.linear.y
+
             control_msg.twist.angular.z = angular
             self.control_publisher.publish(control_msg)
-            
+
             heartbeat_message = Heartbeat()
             heartbeat_message.in_teleop = bool(in_teleop)
             heartbeat_message.e_stopped = bool(e_stopped)
             self.heartbeat_publisher.publish(heartbeat_message)
-            
+
 
 
 def main(args=None):