保存dance测试文件

src/robot_dance/scripts/robot_dance.py

@@ -3,40 +3,81 @@
 '''
 Author: adam_zhuo
 Date: 2021-01-08 15:39:22
-LastEditTime: 2021-01-18 17:50:27
+LastEditTime: 2021-01-20 13:29:54
 LastEditors: Please set LastEditors
 Description: In User Settings Edit
 FilePath: /blackTornadoRobot/src/robot_dance/scripts/robot_dance.py
 '''
+import os
+import threading
 import rospy
 import librosa
 import librosa.display
 import matplotlib.pyplot as plt
 import numpy as np
+from geometry_msgs.msg import Twist
 
 class Dance:
 
     def __init__(self):
         self.path = "/home/tianbot/lucky.mp3"
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+        self.twist = Twist()
+
+    def play_run(self):
+        os.system("play -q --multi-threaded /home/tianbot/lucky.mp3")
+
+    def map_rate(self,X, to_min, to_max):
+        x_min=min(X)
+        x_max=max(X)
+        return list([round(to_min+((to_max - to_min) / (x_max - x_min)) * (i - x_min),3) for i in X])
     
     def plot(self):
-        y, sr = librosa.load(self.path, sr=None)
-        print("sr is",sr)
-        # librosa.samples_to_time(np.arange(0, 10000, sr))
-        print("time is ",len(y)/sr)
+        # play_thread = threading.Thread(target=self.play_run)
+        # play_thread.start()
+        
+        y, sr = librosa.load(self.path)
         o_env = librosa.onset.onset_strength(y=y, sr=sr,
-                                             hop_length=44100,
-                                             aggregate=np.median)
-        print("o_env is ",o_env)
-        # print("sr is",sr)
-        # print("onset shape is",np.shape(onset_env))
-        # dby = librosa.amplitude_to_db(y, ref=1.0)
-        # print(sr)
-        # plt.figure()
-        # librosa.display.waveplot(dby, sr)
-        # plt.title('lucky wavform')
-        # plt.show()
+                                             hop_length=sr,
+                                             aggregate=np.max)
+        times = librosa.times_like(o_env, sr=sr,hop_length=sr)
+
+        max_num = len(y)/sr
+        tempo_array = [0]
+        for num in range(1,max_num+1):
+            y, sr = librosa.load(self.path,offset=num-1,duration=1)
+            onset_env = librosa.onset.onset_strength(y, sr=sr)
+            tempo = librosa.beat.tempo(onset_envelope=onset_env, sr=sr)
+            tempo_array.append(tempo[0])
+        linear_map_array = self.map_rate(tempo_array,0.05,0.3)
+        angular_map_array = self.map_rate(tempo_array,0.5,2.8)
+        time_map_array = self.map_rate(o_env,0.5,1.5)
+    
+        tmp_time = 0
+        for i in range(0,max_num):
+            diff = time_map_array[i] - tmp_time
+            tmp_time = time_map_array[i]
+            
+
+        
+        # onset_env = librosa.onset.onset_strength(y, sr=sr)
+        # tempo = librosa.beat.tempo(onset_envelope=onset_env, sr=sr)
+        # print((tempo))
+    
+    def pub_twist(
+        self,
+        linear_x=0.1,
+        linear_y=0.1,
+        angular_z=0.8):
+        self.twist.linear.x = linear_x
+        self.twist.linear.y = linear_y
+        self.twist.angular.z = angular_z
+        self.cmd_vel_pub.publish(self.twist)
+
+        
 
 if __name__ == '__main__':
+    # rospy.init_node('robot_dance')
     dance = Dance()
-    dance.plot()
+    dance.plot()
+    rospy.spin()

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -15,7 +15,6 @@ import rospy
 import time
 import roslib; roslib.load_manifest('ros_arduino_python')
 import dynamic_reconfigure.client
-import queue
 
 from math import sin, cos, pi, sqrt
 import numpy as np
@@ -111,7 +110,7 @@ class BaseController:
         self.correction_yaw_flag = 0
         self.last_yaw_data = 0
         self.now_yaw_data = 0
-        self.initial_odom_yaw_data = self.get_yaw()
+        # self.initial_odom_yaw_data = self.get_yaw()
 
         self.debugPID   = rospy.get_param('~debugPID', False)
 
@@ -322,8 +321,8 @@ class BaseController:
             self.pose_y += delta_y
             self.th += delta_th
 
-            self.now_odom_yaw_data = self.get_yaw()
-            self.th = self.now_odom_yaw_data - self.initial_odom_yaw_data
+            # self.now_odom_yaw_data = self.get_yaw()
+            # self.th = self.now_odom_yaw_data - self.initial_odom_yaw_data
 
             quaternion   = Quaternion()
             quaternion.x = 0.0
@@ -480,21 +479,21 @@ class BaseController:
         y  = req.linear.y  # m/s
         th = req.angular.z # rad/s
 
-        if x != 0 and y == 0 and th == 0:
-            if self.correction_yaw_flag < self.correction_yaw_flag_timeout:
-                self.stop()
-                self.last_yaw_data = self.get_yaw()
-                self.correction_yaw_flag += 1
-                return
-            else:
-                self.now_yaw_data = self.get_yaw()
-                err_yaw_data = self.now_yaw_data - self.last_yaw_data
-                if abs(err_yaw_data) > self.correction_forward_err_yaw_data:
-                    th = self.pid_controller.update(err_yaw_data)
-                    #print("err_yaw_data is ",err_yaw_data,"th is ",th)
-        else:
-            self.correction_yaw_flag = 0
-            self.reset_pid_controller()  
+        # if x != 0 and y == 0 and th == 0:
+        #     if self.correction_yaw_flag < self.correction_yaw_flag_timeout:
+        #         self.stop()
+        #         self.last_yaw_data = self.get_yaw()
+        #         self.correction_yaw_flag += 1
+        #         return
+        #     else:
+        #         self.now_yaw_data = self.get_yaw()
+        #         err_yaw_data = self.now_yaw_data - self.last_yaw_data
+        #         if abs(err_yaw_data) > self.correction_forward_err_yaw_data:
+        #             th = self.pid_controller.update(err_yaw_data)
+        #             #print("err_yaw_data is ",err_yaw_data,"th is ",th)
+        # else:
+        #     self.correction_yaw_flag = 0
+        #     self.reset_pid_controller()  
 
         #Inverse Kinematic
         tmpX = sqrt(3)/2.0