#!/usr/bin/env python #1
# -*- coding: utf-8 -*- #2

import os #4
import rospy #5
from puppy_control.srv import SetRunActionName, SetRunActionNameRequest #6
from puppy_control.msg import Velocity, Pose, Gait #7
import time #8
import math #9
import subprocess #10

class PuppyControlNode(object): #12
    def __init__(self): #13
        # Stop node initialization(不再初始化节点) #14
        # rospy.init_node('puppy_control_node', anonymous=False) #15
        
        # Publisher(发布者) #17
        self.pose_publisher = rospy.Publisher('/puppy_control/pose', Pose, queue_size=10) #18
        self.gait_publisher = rospy.Publisher('/puppy_control/gait', Gait, queue_size=10) #19
        self.velocity_publisher = rospy.Publisher('/puppy_control/velocity', Velocity, queue_size=10) #20
        
        # Service Client (服务客户端) #22
        rospy.wait_for_service('/puppy_control/runActionGroup') #23
        try: #24
            self.run_action_group_srv = rospy.ServiceProxy('/puppy_control/runActionGroup', SetRunActionName) #25
        except rospy.ServiceException as e: #26
            rospy.logerr("Service initialization failed: %s", e) #27
        
    def set_run_action(self, action_name): #29
        try: #30
            req = SetRunActionNameRequest() #31
            req.name = f'{action_name}.d6ac' #32
            req.wait = True #33
            self.run_action_group_srv.call(req) #34
        except rospy.ServiceException as e: #35
            rospy.logerr("Service call failed: %s", e) #36

    def set_move(self, x=0.00, y=0.0, yaw_rate=0.0): #38
        velocity_msg = Velocity() #39
        velocity_msg.x = x #40
        velocity_msg.y = y #41
        velocity_msg.yaw_rate = yaw_rate #42
        self.velocity_publisher.publish(velocity_msg) #43

    def forward(self): #45
        self.set_move(x=5.0) #46
        time.sleep(2) #47
        self.set_move(x=0.0) #48

    def back(self): #50
        self.set_move(x=-5.0) #51
        time.sleep(2) #52
        self.set_move(x=0.0) #53

    def turn_left(self): #55
        self.set_move(x=-5.0, yaw_rate=math.radians(-30)) #56
        time.sleep(2) #57
        self.set_move(x=0.0, yaw_rate=math.radians(0)) #58

    def turn_right(self): #60
        self.set_move(x=-5.0, yaw_rate=math.radians(30)) #61
        time.sleep(2) #62
        self.set_move(x=0.0, yaw_rate=math.radians(0)) #63
        
    def man(self): #65
        try: #66
            subprocess.run( #67
                ["paplay", #68
                "/home/ubuntu/puppypi/src/large_models/scripts/resources/audio/man.wav"], #69
                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, check=True #70
            ) #71
        except subprocess.CalledProcessError as e: #72
            rospy.logerr("播放 man.wav 失败: %s", e) #73

    def woman(self): #75
        try: #76
            subprocess.run( #77
                ["paplay", #78
                "/home/ubuntu/puppypi/src/large_models/scripts/resources/audio/schoolgirl.wav"], #79
                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, check=True #80
            ) #81
        except subprocess.CalledProcessError as e: #82
            rospy.logerr("播放 schoolgirl.wav 失败: %s", e) #83

    def init(self): #85
        self.set_run_action('stand') #86

    def boxing(self): #88
        self.set_run_action('boxing') #89

    def bow(self): #91
        self.set_run_action('bow') #92

    def push_up(self): #94
        self.set_run_action('push-up') #95

    def shake_hands(self): #97
        self.set_run_action('shake_hands') #98

    def shake_hand(self): #100
        self.set_run_action('shake_hand') #101

    def lie_down(self): #103
        self.set_run_action('lie_down') #104

    def moonwalk(self): #106
        self.set_run_action('moonwalk') #107

    def kick_ball_left(self): #109
        self.set_run_action('kick_ball_left') #110

    def kick_ball_right(self): #112
        self.set_run_action('kick_ball_right') #113

    def nod(self): #115
        self.set_run_action('nod') #116

    def wave(self): #118
        self.set_run_action('wave') #119

    def temp(self): #121
        self.set_run_action('temp') #122

    def stand(self): #124
        self.set_run_action('stand') #125

    def sit(self): #127
        self.set_run_action('sit') #128
        
    def kick_ball(self, color='red'): #130
        try: #131
            subprocess.run(['rosrun', 'example', 'kick_ball_demo', color], #132
                           stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, check=True) #133
        except subprocess.CalledProcessError as e: #134
            rospy.logerr("执行 kick_ball_demo 失败: %s", e) #135
            
    def visual_patrol(self, color='red'): #137
        try: #138
            subprocess.run(['rosrun', 'example', 'visual_patrol_demo', color], #139
                           stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, check=True) #140
        except subprocess.CalledProcessError as e: #141
            rospy.logerr("执行 visual_patrol_demo 失败: %s", e) #142
