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'
+]