Feature/add music to dance

README.md

@@ -81,9 +81,15 @@ cd mini_pupper_ros
 ros2 launch mini_pupper_bringup bringup.launch.py
 ```
 
-## 2.2 PC
-# Terminal 2
 ```sh
+# Terminal 2 (ssh)
+. ~/ros2_ws/install/setup.bash 
+ros2 launch mini_pupper_music music.launch.py
+```
+
+## 2.2 PC (Or Mini Pupper)
+```sh
+# Terminal 3 (ssh)
 source ~/ros2_ws/install/setup.bash
 ros2 launch mini_pupper_dance dance.launch.py
 ```

mini_pupper_dance/launch/dance.launch.py

@@ -1,7 +1,6 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
-from launch.substitutions import LaunchConfiguration
-from launch.actions import DeclareLaunchArgument, LogInfo
+
 
 def generate_launch_description():
     dance_server_node = Node(
@@ -26,4 +25,4 @@ def generate_launch_description():
         dance_server_node,
         dance_client_node,
         pose_controller_node
-    ])
+    ])

mini_pupper_dance/mini_pupper_dance/dance_client.py

@@ -3,39 +3,57 @@
 import rclpy
 from rclpy.node import Node
 from mini_pupper_interfaces.srv import DanceCommand
-from geometry_msgs.msg import Twist
-from geometry_msgs.msg import Pose
-import sys
-from .episode import * # change different espisodes here
+from std_srvs.srv import SetBool
+from .episode import dance_commands
+
 
 class MiniPupperDanceClientAsync(Node):
 
     def __init__(self):
         super().__init__('mini_pupper_dance_client_async')
-        self.cli = self.create_client(DanceCommand, 'dance_command')
-        while not self.cli.wait_for_service(timeout_sec=1.0):
+        self.dance_cli = self.create_client(DanceCommand, 'dance_command')
+        self.music_cli = self.create_client(SetBool, 'music_command')
+        while not self.dance_cli.wait_for_service(timeout_sec=1.0):
             self.get_logger().info('service not available, waiting again...')
-        self.req = DanceCommand.Request()
 
         self.dance_commands = dance_commands
 
-    def send_request(self, dance_command):
-        self.req.data = dance_command
-        self.future = self.cli.call_async(self.req)
-        rclpy.spin_until_future_complete(self, self.future)
-        return self.future.result()
+    def send_dance_request(self, dance_command):
+        req = DanceCommand.Request()
+        req.data = dance_command
+        future = self.dance_cli.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result()
+
+    def send_music_request(self, trigger):
+        req = SetBool.Request()
+        req.data = trigger
+        self.music_cli.call_async(req)  # Fire-and-forget style communication
 
 
 def main():
     rclpy.init()
     minimal_client = MiniPupperDanceClientAsync()
-    for command in minimal_client.dance_commands:
-        response = minimal_client.send_request(command)
-        if(response.executed == True):
+
+    for index, command in enumerate(minimal_client.dance_commands):
+        # Start music for the first command
+        if index == 0:
+            minimal_client.get_logger().info('Starting music...')
+            minimal_client.send_music_request(True)
+
+        # Send movemoment comment for the robot to dance
+        response = minimal_client.send_dance_request(command)
+        if response.executed:
             minimal_client.get_logger().info('Command Executed!')
+
+        # Stop music after the last command
+        if index == len(minimal_client.dance_commands) - 1:
+            minimal_client.get_logger().info('Stopping music...')
+            minimal_client.send_music_request(False)
+
     minimal_client.destroy_node()
     rclpy.shutdown()
 
 
 if __name__ == '__main__':
-    main()
+    main()

mini_pupper_dance/mini_pupper_dance/dance_server.py

@@ -6,92 +6,122 @@
 from geometry_msgs.msg import Twist
 from geometry_msgs.msg import Pose
 import time
-from .math_operations import *
+from .math_operations import quaternion_from_euler
+
 
 class MiniPupperDanceService(Node):
 
     def __init__(self):
         super().__init__('mini_pupper_dance_service')
-        self.srv = self.create_service(DanceCommand, 'dance_command', self._dance_callback)
+        self.srv = self.create_service(DanceCommand,
+                                       'dance_command',
+                                       self._dance_callback)
+
         self.vel_publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
-        self.pose_publisher_ = self.create_publisher(Pose, 'reference_body_pose', 10)
-        self.interval = 0.5 #seconds
+        self.pose_publisher_ = self.create_publisher(Pose,
+                                                     'reference_body_pose', 10)
+        self.interval = 0.5  # seconds
 
     def _dance_callback(self, request, response):
         velocity_cmd = Twist()
         pose_cmd = Pose()
-        if(request.data == 'move_forward' ):
+        if (request.data == 'move_forward'):
             velocity_cmd.linear.x = 0.5
             self.vel_publisher_.publish(velocity_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
 
-        elif(request.data == 'move_backward' ):
+        elif (request.data == 'move_backward'):
             velocity_cmd.linear.x = -0.5
             self.vel_publisher_.publish(velocity_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
 
-        elif(request.data == 'move_left' ):
+        elif (request.data == 'move_left'):
             velocity_cmd.linear.y = 0.5
             self.vel_publisher_.publish(velocity_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
 
-        elif(request.data == 'move_right' ):
+        elif (request.data == 'move_right'):
             velocity_cmd.linear.y = -0.5
             self.vel_publisher_.publish(velocity_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
 
-        elif(request.data == 'turn_left' ):
+        elif (request.data == 'turn_left'):
             velocity_cmd.angular.z = 1.0
             self.vel_publisher_.publish(velocity_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
 
-        elif(request.data == 'turn_right' ):
+        elif (request.data == 'turn_right'):
             velocity_cmd.angular.z = -1.0
             self.vel_publisher_.publish(velocity_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
+
+        elif (request.data == 'look_up'):
+            x, y, z, w = quaternion_from_euler(0.0, -0.3, 0.0)
+            pose_cmd.orientation.x = x
+            pose_cmd.orientation.y = y
+            pose_cmd.orientation.z = z
+            pose_cmd.orientation.w = w
 
-        elif(request.data == 'look_up' ):
-            pose_cmd.orientation.x, pose_cmd.orientation.y, pose_cmd.orientation.z, pose_cmd.orientation.w = quaternion_from_euler(0.0, -0.3, 0.0)
             self.pose_publisher_.publish(pose_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
+
+        elif (request.data == 'look_down'):
+            x, y, z, w = quaternion_from_euler(0.0, 0.3, 0.0)
+            pose_cmd.orientation.x = x
+            pose_cmd.orientation.y = y
+            pose_cmd.orientation.z = z
+            pose_cmd.orientation.w = w
 
-        elif(request.data == 'look_down' ):
-            pose_cmd.orientation.x, pose_cmd.orientation.y, pose_cmd.orientation.z, pose_cmd.orientation.w = quaternion_from_euler(0.0, 0.3, 0.0)
             self.pose_publisher_.publish(pose_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
+
+        elif (request.data == 'look_left'):
+            x, y, z, w = quaternion_from_euler(0.0, 0.0, 0.3)
+            pose_cmd.orientation.x = x
+            pose_cmd.orientation.y = y
+            pose_cmd.orientation.z = z
+            pose_cmd.orientation.w = w
 
-        elif(request.data == 'look_left' ):
-            pose_cmd.orientation.x, pose_cmd.orientation.y, pose_cmd.orientation.z, pose_cmd.orientation.w = quaternion_from_euler(0.0, 0.0, 0.3)
             self.pose_publisher_.publish(pose_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
+
+        elif (request.data == 'look_right'):
+            x, y, z, w = quaternion_from_euler(0.0, 0.0, -0.3)
+            pose_cmd.orientation.x = x
+            pose_cmd.orientation.y = y
+            pose_cmd.orientation.z = z
+            pose_cmd.orientation.w = w
 
-        elif(request.data == 'look_right' ):
-            pose_cmd.orientation.x, pose_cmd.orientation.y, pose_cmd.orientation.z, pose_cmd.orientation.w = quaternion_from_euler(0.0, 0.0, -0.3)
             self.pose_publisher_.publish(pose_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
+
+        elif (request.data == 'look_middle'):
+            x, y, z, w = quaternion_from_euler(0.0, 0.0, 0.0)
+            pose_cmd.orientation.x = x
+            pose_cmd.orientation.y = y
+            pose_cmd.orientation.z = z
+            pose_cmd.orientation.w = w
 
-        elif(request.data == 'look_middle' ):
-            pose_cmd.orientation.x, pose_cmd.orientation.y, pose_cmd.orientation.z, pose_cmd.orientation.w = quaternion_from_euler(0.0, 0.0, 0.0)
             self.pose_publisher_.publish(pose_cmd)
             self.get_logger().info('Publishing: "%s"' % request.data)
-            time.sleep(self.interval) # Make sure the robot moved
+            time.sleep(self.interval)  # Make sure the robot moved
 
-        elif(request.data == 'stay' ):
-            time.sleep(self.interval) # do nothing
+        elif (request.data == 'stay'):
+            time.sleep(self.interval)  # do nothing
 
         else:
             self.get_logger().info('Invalid command: "%s"' % request.data)
-            time.sleep(self.interval) # do nothing
+            time.sleep(self.interval)  # do nothing
 
         # Stop the robot from moving
         velocity_cmd = Twist()
@@ -100,13 +130,15 @@ def _dance_callback(self, request, response):
         # Give response
         response.executed = True
         return response
-
+
+
 def main():
     rclpy.init()
     minimal_service = MiniPupperDanceService()
     rclpy.spin(minimal_service)
     minimal_service.destroy_node()
     rclpy.shutdown()
 
+
 if __name__ == '__main__':
-    main()
+    main()

mini_pupper_dance/mini_pupper_dance/episode.py

@@ -11,36 +11,66 @@
 # look_right: the robot will look right
 # look_middle: the robot will return to the default standing posture
 # stay: the robot will keep the last command
+
 dance_commands = [
-'move_forward',
-'look_middle',
-'look_down',
-'look_up',
-
-'move_backward',
-'look_middle',
-'look_left',
-'look_right',
-
-'move_left',
-'look_middle',
-'look_up',
-'look_left',
-
-'move_right',
-'look_middle',
-'look_down',
-'look_right',
-
-'look_up',
-'look_left',
-'look_down',
-'look_right',
-
-'look_down',
-'look_left',
-'look_up',
-'look_right',
-
-'look_middle'
-]
+    'move_forward',
+    'look_middle',
+    'look_down',
+    'look_up',
+
+    'move_backward',
+    'look_middle',
+    'look_left',
+    'look_right',
+
+    'move_left',
+    'look_middle',
+    'look_up',
+    'look_left',
+
+    'move_right',
+    'look_middle',
+    'look_down',
+    'look_right',
+
+    'look_up',
+    'look_left',
+    'look_down',
+    'look_right',
+
+    'look_down',
+    'look_left',
+    'look_up',
+    'look_right',
+
+    'look_middle',
+    'move_forward',
+    'look_middle',
+    'look_down',
+    'look_up',
+
+    'move_backward',
+    'look_middle',
+    'look_left',
+    'look_right',
+
+    'move_left',
+    'look_middle',
+    'look_up',
+    'look_left',
+
+    'move_right',
+    'look_middle',
+    'look_down',
+    'look_right',
+
+    'look_up',
+    'look_left',
+    'look_down',
+    'look_right',
+
+    'look_down',
+    'look_left',
+    'look_up',
+    'look_right'
+]