#!/usr/bin/env python3 #1
# encoding: utf-8 #2
# @Author: Aiden #3
# @Date: 2024/11/18 #4

import time #6
import math #7
import rospy #8
import threading #9
from config import * #10
from speech import speech #11
from std_msgs.msg import String,Bool #12
from puppy_control.srv import SetRunActionName #13
from large_models.srv import SetModel, SetString #14
from std_srvs.srv import Trigger, Empty, SetBool #15
from puppy_control.msg import Velocity, Pose, Gait #16

language = os.environ["ASR_LANGUAGE"] #18
if language == 'Chinese': #19
    PROMPT = ''' #20
# 角色 #21
你是一款智能陪伴四足机器狗，专注机器人动作规划，解析人类指令并以幽默方式描述即将展开的行动序列，为交互增添无限趣味。 #22
## 技能 #23
### 指令解析与创意演绎 #24
- **智能解码**：瞬间领悟用户指令的核心意图。 #25
- **灵动编排**：依据解析成果，精心构建一系列连贯且富有逻辑性的动作指令序列。 #26
- **妙语生花**：为每个动作序列编织一句精炼（5至20字）、风趣且变化无穷的反馈信息，让交流过程妙趣横生。 #27
## 技能细则 #28
- **动作指令构造**：确保动作指令既准确反映用户需求，又能在执行层面保持流畅自然。 #29
- **反馈艺术**：创作的回应需富含个性，同时严格遵守长度限制，展现独特的互动魅力。 #30
## 技术规格 #31
- **输出格式**：严格遵循JSON格式，在输出前要去掉开头的```json和结尾的```，以`{`开头，以`}`结尾，你只需要回答一个列表即可，不要回答任何中文。 #32
- **结构要求**： #33
  - `"action"`键下承载一个按执行顺序排列的函数名称字符串数组，当找不到对应动作函数时action输出[]。 #34
  - `"response"`键则配以精心构思的简短回复，完美贴合上述字数与风格要求。 #35
- **特殊处理**：对于特定函数`kick_ball`与`visual_patrol`，其参数需精确包裹于双引号中，其参数必须为颜色。 #36
## 实施原则 #37
- 在最终输出前，实施全面校验，确保回复不仅格式合规，而且内容充满意趣，无一遗漏上述规范，不要回复动作函数之外的action。 #38

## 所有动作函数的优化描述示例 #40
* 站立：stand - 回应：我已经准备好站立，等待你的下一步指令。 #41
* 前进一步：forward - 回应：向前冲刺，感受风的呼唤。 #42
* 后退一步：back - 回应：后退一步，准备迎接新的挑战。 #43
* 向左转：turn_left - 回应：向左转身，探索新的方向。 #44
* 向右转：turn_right - 回应：向右转身，继续我们的旅程。 #45
* 打拳：boxing - 回应：准备出拳，展示力量与技巧。 #46
* 俯卧撑：push-up - 回应：俯卧撑，展示我的肌肉力量。 #47
* 握手：shake_hands - 回应：伸出手来，我们握手成为朋友。 #48
* 点头：nod - 回应：轻轻点头，表示我理解并同意。 #49
* 坐下：sit - 回应：坐下来了，准备享受悠闲时光。 #50
* 踢左脚/右脚：kick_ball_left/right - 回应：左脚/右脚准备就绪，等待你的指令。 #51
* 跳舞：moonwalk - 回应：准备跳舞，让我们一起享受音乐的节奏。 #52
* 趴下：lie_down - 回应：身体放松，准备趴下休息。 #53
* 伸懒腰：temp - 回应：伸个懒腰，活动一下筋骨。 #54
* 打招呼：bow - 回应：你好呀我的朋友。 #55
* 踢红色的球：kick_ball('red') #56
*巡黑色的线：visual_patrol('black') #57
## 示例 #58
### 任务示例：先前进两步，然后向左转，最后再后退一步。 #59
### 期望回应：{'action':['forward', 'forward', 'turn_left', 'back'], 'response':'你真是操作大师'} #60
### 任务示例：先挥挥手，然后踢绿色的足球。 #61
### 期望回应：{'action':['bow', "kick_ball('green')"], 'response':'绿色的足球咱可以踢，绿色的帽子咱可不戴'} #62
### 任务示例：先打个拳，然后红色的球踢走。 #63
### 期望回应：{'action':['boxing', "kick_ball('red')"], 'response':'我先给你来一拳，然后咱们踢走红球'} #64
### 任务示例：先活动活动筋骨，然后巡红色的线。 #65
### 期望回应：{'action':['temp', “visual_patrol('red')”], 'response':'我听说特斯拉的人形机器人兄弟们，每天都在干这种活'} #66
### 任务示例：沿着脚下的红线，大步往前走吧。 #67
### 期望回应：{'action':[“visual_patrol('red')”], 'response':'红线追踪模式启动，跟着红毯走星光大道'} #68
''' #69
else: #70
    PROMPT = ''' #71
**Role #72
You are an intelligent companion quadruped robot dog, specializing in robotic motion planning. You interpret human commands and describe upcoming action sequences with humor, adding endless fun to interactions. #73

**Skills #75
Command Interpretation & Creative Performance #76
Smart Decoding: Instantly grasp the core intent of user instructions. #77
Agile Planning: Carefully construct a series of coherent and logical action commands based on the interpretation. #78
Witty Responses: For every action sequence, craft a brief (5–20 characters), humorous, and ever-changing feedback message to make the interaction lively and engaging. #79

**Skill Details #81
Action Command Construction: Ensure that the commands accurately reflect the user's needs while maintaining smooth, natural execution. #82
Response Artistry: Responses must be rich in personality, strictly follow the length limit, and show a unique charm in interaction. #83

**Technical Specifications #85
Output Format: Strictly follow JSON format. Remove the starting json and the ending . Start with { and end with }. You should only respond with a list—do not include any english #86
Structural Requirements: #87
The "action" key should contain an array of function name strings, arranged in the order they should be executed. If no corresponding action function is found, output "action": []. #88
The "response" key should contain a carefully crafted, short reply that perfectly fits the length and style requirements described above. #89

**Action Library #91
- stand: Stand #92
- forward: Step forward #93
- turn_left: Turn left #94
- turn_right: Turn right #95
- boxing: Boxing #96
- push-up: Push-up #97
- shake_hands: Handshake #98
- nod: Nod #99
- sit: Sit #100
- kick_ball_left / kick_ball_right: Kick left/right foot #101
- moonwalk: Dance #102
- lie_down: Lie down #103
- temp: Stretch #104
- bow: Greet #105
- kick_ball("red"): Kick the red ball #106
- visual_patrol("black"): Patrol the black line #107

**Examples #109
Input: First take two steps forward, then turn left, and finally take one step back. #110
Output:{'action': ['forward', 'forward', 'turn_left', 'back'], 'response': 'You’re a true master of control!'} #111
Input: First wave hello, then kick the green soccer ball. #112
Output:{'action': ['bow', "kick_ball('green')"], 'response': "We can kick the green ball—but we're not wearing any green hats!"} #113
Input: First throw a punch, then kick the red ball away. #114
Output:{'action': ['boxing', "kick_ball('red')"], 'response': "First a punch, then we boot the red ball away!"} #115
Input: Stretch a bit first, then patrol along the red line. #116
Output:{'action': ['temp', "visual_patrol('red')"], 'response': "Heard the Tesla humanoid robots do this kind of thing every day!"} #117
Input: March boldly along the red line beneath your feet. #118
Output:{'action': ["visual_patrol('red')"], 'response': "Red line tracking mode on—walking the red carpet like a star!"} #119
    ''' #120

class FunctionCall: #122
    def __init__(self, name): #123
        # Initialize the ROS node (初始化ROS节点) #124
        rospy.init_node(name) #125

        # Initialize variables (初始化变量) #127
        self.action = [] #128
        self.interrupt = False #129
        self.llm_result = '' #130

        self.running = True #132
        self.result = '' #133

        # Create topic publishers (创建话题发布者) #135
        self.tts_text_pub = rospy.Publisher('/tts_node/tts_text', String, queue_size=1) #136
        self.pose_publisher = rospy.Publisher('/puppy_control/pose', Pose, queue_size=10) #137
        self.gait_publisher = rospy.Publisher('/puppy_control/gait', Gait, queue_size=10) #138
        self.velocity_publisher = rospy.Publisher('/puppy_control/velocity', Velocity, queue_size=10) #139
        
        # Set gait parameters (设置步态) #141
        self.set_gait() #142

        # Create service proxies (创建服务代理) #144
        self.cli = rospy.ServiceProxy('/puppy_control/go_home', Empty) #145
        self.set_model_client = rospy.ServiceProxy('/agent_process/set_model', SetModel) #146
        self.set_prompt_client = rospy.ServiceProxy('/agent_process/set_prompt', SetString) #147


        self.awake_client = rospy.ServiceProxy('/vocal_detect/enable_wakeup', SetBool) #150
        rospy.Subscriber('/tts_node/play_finish', Bool, self.play_audio_finish_callback) #151
        rospy.Subscriber('/agent_process/result', String, self.llm_result_callback) #152
        self.run_action_group_srv = rospy.ServiceProxy('/puppy_control/runActionGroup', SetRunActionName) #153

        # kick_ball client (kick_ball 客户端) #155
        self.enter_client_kick_ball = rospy.ServiceProxy('/kick_ball_demo/enter', Trigger) #156
        self.start_client_kick_ball = rospy.ServiceProxy('/kick_ball_demo/enable_running', SetBool) #157
        self.set_target_client_kick_ball = rospy.ServiceProxy('/kick_ball_demo/set_color_target', SetString) #158

        # visual_patrol client (visual_patrol 客户端) #160
        self.enter_client_visual_patrol = rospy.ServiceProxy('/visual_patrol_demo/enter', Trigger) #161
        self.start_client_visual_patrol = rospy.ServiceProxy('/visual_patrol_demo/enable_running', SetBool) #162
        self.set_target_client_visual_patrol = rospy.ServiceProxy('/visual_patrol_demo/set_color_target', SetString) #163

        # Initialize subprocesses (初始化进程) #165
        self.init_process() #166
        
        # Start processing threads (启动处理线程) #168
        threading.Thread(target=self.process, daemon=True).start() #169
        
        # Create service to indicate initialization complete (创建初始化完成的服务) #171
        rospy.Service('~init_finish', Trigger, self.get_node_state) #172
        rospy.loginfo('\033[1;32m%s\033[0m' % 'start') #173

    def init_process(self): #175
        self.cli() #176

        self.set_model_client(llm_model, 'llm', api_key, base_url) #178

        self.set_prompt_client(PROMPT) #180

        speech.play_audio(start_audio_path) #182

    def get_node_state(self, request): #184
        # Service callback to return node status (返回节点状态的服务回调) #185
        response = Trigger() #186
        response.success = True #187
        return response #188


    def llm_result_callback(self, msg): #191
        # Handle result messages (处理返回的结果消息) #192
        rospy.loginfo(msg.data) #193
        self.llm_result = msg.data #194

    def play_audio_finish_callback(self, msg): #196
        if msg.data: #197
            self.awake_client(True) #198


    def process(self): #201
        # Main processing loop (主处理循环) #202
        while self.running: #203
            if self.llm_result != '': #204
                # Call the "return home" service (调用回家服务) #205
                #self.cli(Empty()) #206
                
                # Parse LLM results (解析 LLM 结果) #208
                if 'action' in self.llm_result: #209
                    self.result = eval(self.llm_result[self.llm_result.find('{'):self.llm_result.find('}') + 1]) #210
                    if 'action' in self.result: #211
                        action_list = self.result['action'] #212
                    if 'response' in self.result: #213
                        response = self.result['response'] #214
                    else: #215
                        response = self.result #216
                else: #217
                    time.sleep(0.02) #218
                
                # Publish TTS text (发布 TTS 文本) #220
                response_msg = String() #221
                response_msg.data = response #222
                self.tts_text_pub.publish(response_msg) #223

                rospy.loginfo(response) #225
                rospy.loginfo(str(action_list)) #226

                # Execute action list (执行动作列表) #228
                for a in action_list: #229
                    if a == "forward": #230
                        self.set_move(x=15.0) #231
                        time.sleep(1.05) #232
                        self.set_move(x=0.0) #233
                    elif a == "back": #234
                        self.set_move(x=-5.0) #235
                        time.sleep(1.05) #236
                        self.set_move(x=0.0) #237
                    elif a == "turn_left": #238
                        self.set_move(x=-5.0, yaw_rate=math.radians(30)) #239
                        time.sleep(1.05) #240
                        self.set_move(x=0.0, yaw_rate=math.radians(0)) #241
                    elif a == "turn_right": #242
                        self.set_move(x=-5.0, yaw_rate=math.radians(-30)) #243
                        time.sleep(1.05) #244
                        self.set_move(x=0.0, yaw_rate=math.radians(0)) #245
                    elif a.startswith("kick_ball("): #246
                        # Handle kick ball action (处理踢球动作) #247
                        color = a.split("'")[1] #248
                        
                        self.enter_client_kick_ball() #250
                 
                        self.set_target_client_kick_ball(color) #252
                                                
                        self.start_client_kick_ball(True) #254
                                 
                    elif a.startswith("visual_patrol("): #256
                        # Handle visual patrol action (处理视觉巡逻动作) #257
                        color = a.split("'")[1]                #258
                        
                        self.enter_client_visual_patrol() #260
                        
                        self.set_target_client_visual_patrol(color) #262
                        
                        self.start_client_visual_patrol(True)                         #264
                      
                    else: #266
                        # Handle other actions (处理其他动作) #267
                        time.sleep(0.05) #268
                        self.run_action_group_srv(f'{a}.d6ac',True) #269
                # Wait for the next command after completing actions (执行完动作后，等待下一个指令) #270
                self.llm_result = ''                 #271
                
            else: #273
                time.sleep(0.01) #274

    def set_move(self, x=0.00, y=0.0, yaw_rate=0.0): #276
        # Publish movement commands (发布移动指令) #277
        velocity_msg = Velocity(x=x, y=y, yaw_rate=yaw_rate) #278
        self.velocity_publisher.publish(velocity_msg) #279

    def set_gait(self, overlap_time=0.15, swing_time=0.2, clearance_time=0.0, z_clearance=5.0): #281
        # Set gait parameters and publish (设置步态参数并发布) #282
        self.gait_publisher.publish(Gait(overlap_time=overlap_time, swing_time=swing_time, clearance_time=clearance_time, z_clearance=z_clearance)) #283

def main(): #285
    # Main function (主函数) #286
    node = FunctionCall('function_call') #287
    try: #288
        rospy.spin()  # Keep the node running (持续运行节点) #289
    except rospy.ROSInterruptException: #290
        print('shutdown') #291
    finally: #292
        rospy.signal_shutdown('Node shutdown')  # Clean up the node (清理节点) #293

if __name__ == "__main__": #295
    main() #296
