From ba0549f9f6d31ae23e82f74563e7bf19a7d39936 Mon Sep 17 00:00:00 2001 From: Sara Koshi <126622391+RoboticLions@users.noreply.github.com> Date: Fri, 24 Jan 2025 02:47:02 +0000 Subject: [PATCH] took smoothing out of auto and added to teleop --- .../xdrive_controller.py | 12 +++---- .../all_seaing_driver/onshore_node.py | 11 ++++-- .../all_seaing_driver/rover_lora_combined.py | 35 ++++++++++++------- 3 files changed, 37 insertions(+), 21 deletions(-) diff --git a/all_seaing_controller/all_seaing_controller/xdrive_controller.py b/all_seaing_controller/all_seaing_controller/xdrive_controller.py index e1621f1b..5461c8d9 100755 --- a/all_seaing_controller/all_seaing_controller/xdrive_controller.py +++ b/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) diff --git a/all_seaing_driver/all_seaing_driver/onshore_node.py b/all_seaing_driver/all_seaing_driver/onshore_node.py index d0c60750..4d187a6a 100755 --- a/all_seaing_driver/all_seaing_driver/onshore_node.py +++ b/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) diff --git a/all_seaing_driver/all_seaing_driver/rover_lora_combined.py b/all_seaing_driver/all_seaing_driver/rover_lora_combined.py index f4181c8c..feaf2c6c 100755 --- a/all_seaing_driver/all_seaing_driver/rover_lora_combined.py +++ b/all_seaing_driver/all_seaing_driver/rover_lora_combined.py @@ -16,18 +16,22 @@ 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 @@ -35,32 +39,37 @@ 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):