[ADD] Yaml configuration for unlimited number of wave components, base signal

fft/fft/fft.py

@@ -11,7 +11,7 @@ class MinimalSubscriber(Node):
 
     def __init__(self):
         super().__init__('minimal_subscriber')
-        self.subscription = self.create_subscription(Float32,'sine_wave', self.listener_callback, 10)
+        self.subscription = self.create_subscription(Float32,'sample_wave', self.listener_callback, 10)
         self.subscription
         self.publisher_ = self.create_publisher(Float32, 'check', 10)
         

sample_signal/config/sample_signal.yaml

-/**/sample_signal1:
-  ros__parameters:
-    frequency: 2.0
-    amplitude: 40.0
-
-/**/sample_signal2:
-  ros__parameters:
-    frequency: 5.0
-    amplitude: 20.0    
-
-/**/sample_signal3:
-  ros__parameters:
-    frequency: 10.0
-    amplitude: 10.0                       
-
-
-/**/sample_signal4:
-  ros__parameters:
-    frequency: 20.0
-    amplitude: 5.0
\ No newline at end of file
+/**/sample_signal:
+  ros__parameters:
+    publisher_frequency:              50.0                     # Hz
+    base_signal_initial_value:        0.0                      
+    base_signal_step:                 0.1
+    sine_wave_components:             [ 2.0,  40.0, 0.0,       # Frequency, amplitude, phase shift(radians) pairs
+                                        10.0, 10.0, 1.57,      # Add as much as needed
+                                        #5.0,  3.0 
+                                      ]
\ No newline at end of file

sample_signal/config/sine_wave.yaml

-/**/sin:
-  ros__parameters:
-    frequency: 2.0
-    amplitude: 40.0
-    sample_time: 0.02
\ No newline at end of file

sample_signal/launch/launch.py

@@ -8,31 +8,9 @@ def generate_launch_description():
         
     node1=Node(
         package = 'sample_signal',
-        name = 'sample_signal1',
+        name = 'sample_signal',
         executable = 'sample_signal',
         parameters = [os.path.join(get_package_share_directory('sample_signal'), 'config', 'sample_signal.yaml')]
     )
-    node2=Node(
-        package = 'sample_signal',
-        name = 'sample_signal2',
-        executable = 'sample_signal',
-        parameters = [os.path.join(get_package_share_directory('sample_signal'), 'config', 'sample_signal.yaml')]
-    )
-    node3=Node(
-        package = 'sample_signal',
-        name = 'sample_signal3',
-        executable = 'sample_signal',
-        parameters = [os.path.join(get_package_share_directory('sample_signal'), 'config', 'sample_signal.yaml')]
-    )
-    node4=Node(
-        package = 'sample_signal',
-        name = 'sample_signal4',
-        executable = 'sample_signal',
-        parameters = [os.path.join(get_package_share_directory('sample_signal'), 'config', 'sample_signal.yaml')]
-    )
-
     ld.add_action(node1)
-    # ld.add_action(node2)
-    # ld.add_action(node3)
-    # ld.add_action(node4)
     return ld
\ No newline at end of file

sample_signal/sample_signal/sample_signal.py

-import rclpy
 from rclpy.node import Node
-import numpy as np
 from std_msgs.msg import Float32
 
+import numpy as np
+import rclpy
 
-class SineWavePublisher(Node):
+
+class SampleWavePublisher(Node):
 
     def __init__(self):
-        super().__init__('sine_wave_publisher')
-        self.declare_parameter('frequency', 10.0)
-        self.declare_parameter('amplitude', 10.0)
-        self.declare_parameter('sample_time', 0.01)
-        self.publisher_ = self.create_publisher(Float32, 'sine_wave', 10)
-        self.timer_period = self.get_parameter('sample_time').get_parameter_value().double_value  # seconds
+        super().__init__('sample_wave_publisher')
+        
+        self.declare_parameter('sine_wave_components',          [2.0, 40.0, 1.57])
+        self.declare_parameter('publisher_frequency',            50.0)
+        self.declare_parameter('base_signal_initial_value',      0.0)
+        self.declare_parameter('base_signal_step',               0.1)
+
+        self.timer_period = 1/self.get_parameter('publisher_frequency').get_parameter_value().double_value
+        self.sine_wave_components = self.get_parameter('sine_wave_components').get_parameter_value().double_array_value
+        self.base_signal_value = self.get_parameter('base_signal_initial_value').get_parameter_value().double_value
+
+        self.publisher_ = self.create_publisher(Float32, 'sample_wave', 10)
         self.timer = self.create_timer(self.timer_period, self.timer_callback)
-        self.i = 0
-        self.counter = 0
-        self.frequency = self.get_parameter('frequency').get_parameter_value().double_value
-        self.amplitude = self.get_parameter('amplitude').get_parameter_value().double_value
+        self.time_step = 0.0
 
     def timer_callback(self): 
         msg = Float32() 
-        msg.data = self.amplitude * np.cos(2 * np.pi * self.frequency * self.i * self.timer_period) + \
-            self.amplitude * np.cos(2 * np.pi * self.frequency * self.i * self.timer_period*2) + \
-            self.amplitude * np.sin(2 * np.pi * self.frequency * self.i * self.timer_period/2) + \
-            self.amplitude * np.sin(2 * np.pi * self.frequency * self.i * self.timer_period*2.5)
+        msg.data = 0.0
+
+        for i in range(0, len(self.sine_wave_components), 3): 
+            sine_frequency = self.sine_wave_components[i] 
+            sine_amplitude = self.sine_wave_components[i + 1] 
+            phase_shift =    self.sine_wave_components[i + 2] 
+            msg.data += sine_amplitude * np.cos(2 * np.pi * sine_frequency * self.time_step * self.timer_period + phase_shift) + self.base_signal_value
+            
         self.publisher_.publish(msg) 
-        self.i += 1
+        self.time_step += 1.0
+        self.base_signal_value += self.get_parameter('base_signal_step').get_parameter_value().double_value
 
 
 def main(args=None):
     rclpy.init(args=args)
-
-    sine_wave_publisher = SineWavePublisher()
-
-    rclpy.spin(sine_wave_publisher)
-
-    sine_wave_publisher.destroy_node()
+    sample_wave_publisher = SampleWavePublisher()
+    rclpy.spin(sample_wave_publisher)
+    sample_wave_publisher.destroy_node()
     rclpy.shutdown()