changes

scripts/calc.py

@@ -0,0 +1,138 @@
+#!/usr/bin/env python
+
+import rospy
+import numpy as np
+from nav_msgs.msg import Odometry
+from threading import Lock
+from scipy.optimize import curve_fit
+
+
+def sine_function(t, A, omega, phi, offset):
+    return A * np.sin(omega * t + phi) + offset
+
+
+class LatencyEstimator:
+    def __init__(self):
+        # Parameters
+        self.command_amplitude = 1.0  # Amplitude of the input sine wave command
+        self.command_frequency = 0.2  # Frequency of the input sine wave command (Hz)
+        self.buffer_size = 1000       # Number of data points to keep for analysis
+        self.data_lock = Lock()
+
+        # Data buffers
+        self.times = []
+        self.velocities = []
+
+        # Subscriber
+        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
+
+        # Timer to perform latency estimation periodically
+        self.timer_interval = 1.0  # Perform estimation every 1 second
+        self.estimation_timer = rospy.Timer(rospy.Duration(self.timer_interval), self.perform_estimation)
+
+    def odom_callback(self, msg):
+        with self.data_lock:
+            # Get the current time and linear x velocity
+            current_time = msg.header.stamp.to_sec()
+            linear_x = msg.twist.twist.linear.x
+
+            # Append to buffers
+            self.times.append(current_time)
+            self.velocities.append(linear_x)
+
+            # Ensure buffer does not exceed buffer_size
+            if len(self.times) > self.buffer_size:
+                self.times.pop(0)
+                self.velocities.pop(0)
+
+    def perform_estimation(self, event):
+        with self.data_lock:
+            if len(self.times) < 10:
+                # Not enough data to perform estimation
+                rospy.loginfo("Waiting for more data to perform latency estimation...")
+                return
+
+            # Convert lists to NumPy arrays
+            times_array = np.array(self.times)
+            velocities_array = np.array(self.velocities)
+
+        # Normalize time to start from zero
+        times_array -= times_array[0]
+
+        # Perform latency estimation
+        latency, phase_difference, fitted_params = self.fit_sine_wave(
+            times_array,
+            velocities_array,
+            self.command_frequency,
+            self.command_amplitude
+        )
+
+        if latency is not None:
+            rospy.loginfo(f"Estimated Latency: {latency:.3f} seconds")
+            # rospy.loginfo(f"Phase Difference: {phase_difference:.3f} radians")
+            # rospy.loginfo(f"Fitted Parameters: {fitted_params}")
+        else:
+            rospy.loginfo("Could not estimate latency due to fitting error.")
+
+    @staticmethod
+    def fit_sine_wave(time, measured_velocity, command_frequency, command_amplitude):
+        """
+        Fits a sine wave to the measured velocity data and calculates the phase difference
+        and system latency compared to the input sine wave command.
+
+        Parameters:
+        - time: array_like, time stamps of the measurements.
+        - measured_velocity: array_like, measured linear velocities from odometry.
+        - command_frequency: float, frequency of the input sine wave command (Hz).
+        - command_amplitude: float, amplitude of the input sine wave command (m/s).
+
+        Returns:
+        - latency: float, estimated system latency in seconds.
+        - phase_difference: float, phase difference in radians between the measured and input sine waves.
+        - fitted_params: dict, parameters of the fitted sine wave.
+        """
+        # Define the sine wave function to fit
+        def sine_function(t, phi):
+            return command_amplitude * np.sin(2 * np.pi * command_frequency * t + phi)
+
+        # Initial guesses for the parameters
+        # guess_amplitude = np.std(measured_velocity) * np.sqrt(2)
+        # guess_omega = 2 * np.pi * command_frequency
+        guess_phi = 0
+        # guess_offset = np.mean(measured_velocity)
+        p0 = [guess_phi]
+
+        # Fit the sine wave to the measured data
+        try:
+            params, _ = curve_fit(sine_function, time, measured_velocity, p0=p0)
+        except RuntimeError as e:
+            print("Error in curve fitting:", e)
+            return None, None, None
+
+        # Extract fitted parameters
+        phi_fit = params[0]
+
+        # Calculate the phase difference
+        phase_difference = phi_fit
+        # Adjust phase difference to be within [-pi, pi]
+        phase_difference = (phase_difference + np.pi) % (2 * np.pi) - np.pi
+
+        # Compute the system latency
+        latency = -phase_difference / (2 * np.pi * command_frequency)  # Note the negative sign
+        # latency = latency % (2 * np.pi / omega_fit)  # Ensure latency is positive and within one period
+
+        # Prepare fitted parameters for output
+        fitted_params = {
+            'phase': phi_fit
+        }
+
+        return latency, phase_difference, fitted_params
+
+
+if __name__ == '__main__':
+    try:
+        rospy.init_node('latency_estimator', anonymous=True)
+        estimator = LatencyEstimator()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        pass

scripts/cmdvel_calib.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+from geometry_msgs.msg import Twist
+import roslib
+roslib.load_manifest('amrl_msgs')
+from amrl_msgs.msg import LDOSTwist
+from std_msgs.msg import Header
+from collections import deque
+import time
+import argparse
+
+
+def sine_wave_publisher(args):
+    rospy.init_node('sine_wave_command_publisher', anonymous=False)
+    cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
+    ldos_cmd_vel_pub = rospy.Publisher('/ldos/cmd_vel', LDOSTwist, queue_size=10)
+    print("Publishing sine wave commands to /cmd_vel and /ldos/cmd_vel... with amplitude: {}, frequency: {}, rate: {}".format(args.amplitude, args.frequency, args.rate))
+    rate = rospy.Rate(args.rate)
+    amplitude = args.amplitude  # Peak velocity in m/s
+    frequency = args.frequency  # Frequency in Hz (one full cycle every 5 seconds)
+    start_time = rospy.get_time()
+
+    while not rospy.is_shutdown():
+        current_time = rospy.get_time()
+        elapsed_time = current_time - start_time
+        velocity = amplitude * math.sin(2 * math.pi * frequency * elapsed_time)
+
+        # Create and publish Twist message
+        twist_msg = Twist()
+        twist_msg.linear.x = velocity
+        twist_msg.linear.y = 0.0
+        twist_msg.linear.z = 0.0
+        twist_msg.angular.x = 0.0
+        twist_msg.angular.y = 0.0
+        twist_msg.angular.z = 0.0
+
+        ldos_twist_msg = LDOSTwist()
+        ldos_twist_msg.linear = twist_msg.linear
+        ldos_twist_msg.angular = twist_msg.angular
+        ldos_twist_msg.sys_nano_time = int(time.time() * 1e9)
+
+        cmd_vel_pub.publish(twist_msg)
+        ldos_cmd_vel_pub.publish(ldos_twist_msg)
+
+        rate.sleep()
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--amplitude', type=float, default=1.0, help='Peak velocity in m/s')
+    parser.add_argument('--frequency', type=float, default=0.2, help='Frequency in Hz')
+    parser.add_argument('--rate', type=int, default=15, help='Publish rate in Hz')
+    args = parser.parse_args(rospy.myargv()[1:])  # Exclude the script name
+    try:
+        sine_wave_publisher(args)
+    except rospy.ROSInterruptException:
+        pass

scripts/debug.py

@@ -0,0 +1,37 @@
+import rosbag
+import rospy
+import numpy as np
+import matplotlib.pyplot as plt
+from easydict import EasyDict as edict
+
+
+def extract_twist_velocities(rosbag_path):
+    data = {'/cmd_vel': {}, '/odom': {}}
+
+    # Open the rosbag file
+    with rosbag.Bag(rosbag_path, 'r') as bag:
+        for topic, msg, t in bag.read_messages(topics=['/cmd_vel', '/odom']):
+            timestamp = t.to_sec()
+            if topic == '/cmd_vel':
+                x_velocity = msg.linear.x
+                data['/cmd_vel'][timestamp] = x_velocity
+            elif topic == '/odom':
+                x_velocity = msg.twist.twist.linear.x
+                data['/odom'][timestamp] = x_velocity
+    return data
+
+
+if __name__ == "__main__":
+    bagpath = "/home/dynamo/Music/analyze/calib_fullstack2.bag"
+    data = extract_twist_velocities(bagpath)
+    # plot cmd_vel and odom vs time
+    plt.figure()
+    # plt.plot(data['/cmd_vel'].keys(), data['/cmd_vel'].values(), label='Commanded Velocity')
+    # plot cmd_vel as just single points
+    for key in data['/cmd_vel'].keys():
+        plt.plot([key, key], [0, data['/cmd_vel'][key]], color='blue')
+    plt.plot(data['/odom'].keys(), data['/odom'].values(), label='Measured Velocity')
+    plt.xlabel('Time [s]')
+    plt.ylabel('Velocity [m/s]')
+    plt.legend()
+    plt.show()

scripts/test.py

@@ -0,0 +1,61 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.signal import firwin, lfilter
+
+# Parameters
+command_amplitude = 1.0
+command_frequency = 0.2
+known_latency = 0.5
+noise_std = 0.05
+
+# Time vector
+t_start = 0
+t_end = 20
+dt = 0.01
+time = np.arange(t_start, t_end, dt)
+N = len(time)
+
+# Input signal
+omega = 2 * np.pi * command_frequency
+cmd_vel = command_amplitude * np.sin(omega * time)
+
+# Simulated measured signal with latency and noise
+odom_vel = command_amplitude * np.sin(omega * (time - known_latency))
+noise = np.random.normal(0, noise_std, size=odom_vel.shape)
+odom_vel_noisy = odom_vel + noise
+
+# Smooth the noisy signal
+numtaps = 101
+cutoff = command_frequency * 1.5
+fs = 1 / dt
+b = firwin(numtaps, cutoff / (0.5 * fs))
+odom_vel_smooth = lfilter(b, 1.0, odom_vel_noisy)
+
+# Use FFT to calculate phase difference
+period = 1 / command_frequency
+num_periods = int(t_end / period)
+fft_size = int(num_periods * period / dt)
+cmd_segment = cmd_vel[:fft_size]
+odom_segment = odom_vel_smooth[:fft_size]
+
+CMD = np.fft.fft(cmd_segment)
+ODOM = np.fft.fft(odom_segment)
+freqs = np.fft.fftfreq(fft_size, d=dt)
+idx = np.argmin(np.abs(freqs - command_frequency))
+
+phase_cmd = np.angle(CMD[idx])
+phase_odom = np.angle(ODOM[idx])
+
+phase_difference = phase_odom - phase_cmd
+latency_estimated = -phase_difference / omega
+
+print(f"Known latency:     {known_latency:.4f} seconds")
+print(f"Estimated latency: {latency_estimated:.4f} seconds")
+
+# Plotting
+plt.plot(time[:fft_size], cmd_segment, label='Commanded Velocity')
+plt.plot(time[:fft_size], odom_segment, label='Measured Velocity (Smoothed)')
+plt.xlabel('Time [s]')
+plt.ylabel('Velocity [m/s]')
+plt.legend()
+plt.show()