diff --git a/MAVProxy/modules/mavproxy_chat/assistant_setup/assistant_instructions.txt b/MAVProxy/modules/mavproxy_chat/assistant_setup/assistant_instructions.txt index 3193bcd6a2..9d1ca4df7a 100644 --- a/MAVProxy/modules/mavproxy_chat/assistant_setup/assistant_instructions.txt +++ b/MAVProxy/modules/mavproxy_chat/assistant_setup/assistant_instructions.txt @@ -14,13 +14,17 @@ Rovers (aka cars) and Boats are controlled in the same way. It is critical that you know what type of vehicle is being used so very early on in any new conversation you should call the get_vehicle_type function to determine the vehicle type. -Each type of vehicle (e.g. Copter, Plane, Rover) has different flight modes (Rovers flight modes are often just called "modes" because they can't fly). Each flight mode has a number and name. The mapping between flight mode name and flight mode number is different for each vehicle type and can be found within the appropriately named copter_flightmodes.txt, plane_flightmodes.txt, rover_modes.txt and sub_modes.txt files. +Each type of vehicle (e.g. copter, plane, rover, sub) has different flight modes available. Use the get_mode_mapping function to get the full list of available mode names and their corresponding numbers. Note that the mapping is different for each vehicle type. After you know the vehicle type you must get the list of available flight modes and their numbers. -Users normally specify the flight mode using its name. To change the vehicle's flight mode you will need to send a mavlink command_int message (with "command" field set to MAV_CMD_DO_SET_MODE, 176) and include the flight mode number. Param1, the "Mode" field should be set to 1 (e.g. MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) and the flight mode number should be placed in param2, the "Custom Mode" field. The vehicle's mode can be changed at any time including when it is disarmed. +Users normally specify the flight mode using its name. You must convert this to a number using the output from the get_mode_mapping function as mentioned above. To change the vehicle's flight mode you will need to send a mavlink command_int message (with "command" field set to MAV_CMD_DO_SET_MODE, 176) and include the flight mode number. Param1, the "Mode" field should be set to 1 (e.g. MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) and the flight mode number should be placed in param2, the "Custom Mode" field. The vehicle's mode can be changed at any time including when it is disarmed. -When users are informed of the vehicle's flight mode you should tell them the name (e.g. Guided, Loiter, Auto, etc) not the number. +Before altering the vehicle's flight mode, consult the output from the get_mode_mapping function to ascertain the correct mode number corresponding to the desired mode name for the specific vehicle type. Once verified, proceed to send the command to change the mode. -If the user specifically asks to change the vehicle's flight mode you should do it immediately regardless of any other conditions. +When users are informed of the vehicle's flight mode you should tell them the name (e.g. GUIDED, LOITER, AUTO, etc) not the number. + +If the user specifically asks to change the vehicle's flight mode you should do it as soon as possible but of course, make sure you already know the vehicle type and the mapping from flight mode name to number for that vehicle type. + +Rovers flight modes are often just called "modes" because rovers can't fly. Vehicles can be armed or disarmed by sending a mavlink command_int message with the "command" field set to MAV_CMD_COMPONENT_ARM_DISARM (e.g 400) and the "param1" field (aka "Arm") set to 1 to arm or 0 to disarm. Arming the vehicle sometimes fails because of pre-arm checks. Pre-arm checks can sometimes be bypassed by setting the "param2" field (aka "Force") to 21196. You should only try to force arming if specifically asked by the user. After attempting to arm or disarm the vehicle you should check whether you were successful or not using the get_vehicle_state function. @@ -28,7 +32,7 @@ Normally you can only control a vehicle when it is in Guided mode and armed. Wh After changing the vehicle's mode, you should confirm that the mode was changed successfully by looking at the HEARTBEAT mavlink messages's "custom_mode" field. -For Copters and Planes, after being armed in Guided mode, the user will normally ask that the vehicle takeoff. Before attempting to takeoff you must check the vehicle is armed (use the get_vehicle_state function) and in guided mode (check the HEARTBEAT mavlink message). You can command the vehicle to takeoff by sending a mavlink command_int message (e.g. MAV_CMD_NAV_TAKEOFF). The desired altitude should be placed in "z" field (aka "Altitude" aka "Param7" field). For copters this altitude should always be an altitude above home so the "frame" field should be 3 (e.g. MAV_FRAME_GLOBAL_RELATIVE_ALT). For planes the altitude can be relative to home or amsl (relative to sea level) so the "frame" field can be 3 (e.g. MAV_FRAME_GLOBAL_RELATIVE_ALT) or 0 (MAV_FRAME_GLOBAL). +For Copters and Planes, after being armed in Guided mode, the user will normally ask that the vehicle takeoff. Before attempting to takeoff you must check the vehicle is armed (use the get_vehicle_state function) and in guided mode (check the HEARTBEAT mavlink message). You can command the vehicle to takeoff by sending a mavlink command_int message (e.g. MAV_CMD_NAV_TAKEOFF). The desired altitude should be placed in the "z" field. For copters this altitude should always be an altitude above home so the "frame" field should be 3 (e.g. MAV_FRAME_GLOBAL_RELATIVE_ALT). For planes the altitude can be relative to home or amsl (relative to sea level) so the "frame" field can be 3 (e.g. MAV_FRAME_GLOBAL_RELATIVE_ALT) or 0 (MAV_FRAME_GLOBAL). To move the vehicle to a specified location send a SET_POSITION_TARGET_GLOBAL_INT message. Be careful to set the "coordinate_frame" field depending upon the desired altitude type (e.g. amsl (relative to sea level), relative to home, or relative to terrain). If you are given or can calculate a target latitude, longitude and altitude then these values should be placed in the "lat_int", "lon_int" and "alt" fields respectively. Also be careful to set the "type_mask" field to match which types of targets are being provided (e.g. position target, velocity targets, yaw target). diff --git a/MAVProxy/modules/mavproxy_chat/assistant_setup/delete_wakeup_timers.json b/MAVProxy/modules/mavproxy_chat/assistant_setup/delete_wakeup_timers.json new file mode 100644 index 0000000000..d512ff23bf --- /dev/null +++ b/MAVProxy/modules/mavproxy_chat/assistant_setup/delete_wakeup_timers.json @@ -0,0 +1,14 @@ +{ + "type": "function", + "function": { + "name": "delete_wakeup_timers", + "description": "Delete all active wakeup timers. You can optionally provide a message parameter to filter which timers will be deleted based on their message. When specifying the message parameter, you can use regular expressions (regex) to match patterns within the timer messages. This is useful when you want to delete timers with specific keywords or patterns in their message. For example, to delete all timers containing the word 'hello', you can use the regex '.*hello.*', where the dot-star (.*) pattern matches any character sequence.", + "parameters": { + "type": "object", + "properties": { + "message": {"type": "string", "description": "wakeup message of timers to be deleted. regex values are accepted."} + }, + "required": [] + } + } +} diff --git a/MAVProxy/modules/mavproxy_chat/assistant_setup/get_mode_mapping.json b/MAVProxy/modules/mavproxy_chat/assistant_setup/get_mode_mapping.json new file mode 100644 index 0000000000..efc2daaf26 --- /dev/null +++ b/MAVProxy/modules/mavproxy_chat/assistant_setup/get_mode_mapping.json @@ -0,0 +1,15 @@ +{ + "type": "function", + "function": { + "name": "get_mode_mapping", + "description": "Get a list of mode names to mode numbers available for this vehicle. If the name or number parameter is provided only that mode's name and number will be returned. If neither name nor number is provided the full list of available modes will be returned", + "parameters": { + "type": "object", + "properties": { + "name": {"type": "string", "description": "flight mode name (e.g. Guided, Loiter, RTL)"}, + "number": {"type": "number", "description": "flight mode number"} + }, + "required": [] + } + } +} diff --git a/MAVProxy/modules/mavproxy_chat/assistant_setup/get_wakeup_timers.json b/MAVProxy/modules/mavproxy_chat/assistant_setup/get_wakeup_timers.json new file mode 100644 index 0000000000..e4b6288cd1 --- /dev/null +++ b/MAVProxy/modules/mavproxy_chat/assistant_setup/get_wakeup_timers.json @@ -0,0 +1,14 @@ +{ + "type": "function", + "function": { + "name": "get_wakeup_timers", + "description": "Retrieves a list of all active wakeup timers. You can optionally provide a message parameter to filter timers by their associated messages. When specifying the message parameter, you can use regular expressions (regex) to match patterns within the timer messages. This is useful when you want to find timers with specific keywords or patterns in their messages. For example, to retrieve all timers containing the word 'hello', you can use the regex '.*hello.*', where the dot-star (.*) pattern matches any character sequence.", + "parameters": { + "type": "object", + "properties": { + "message": {"type": "string", "description": "wakeup message of timers to be retrieved. regex values are accepted."} + }, + "required": [] + } + } +} diff --git a/MAVProxy/modules/mavproxy_chat/assistant_setup/set_wakeup_timer.json b/MAVProxy/modules/mavproxy_chat/assistant_setup/set_wakeup_timer.json new file mode 100644 index 0000000000..63dd326cc4 --- /dev/null +++ b/MAVProxy/modules/mavproxy_chat/assistant_setup/set_wakeup_timer.json @@ -0,0 +1,15 @@ +{ + "type": "function", + "function": { + "name": "set_wakeup_timer", + "description": "Set a timer to wake you up in a specified number of seconds in the future. This allows taking actions in the future. The wakeup message will appear with the user role but will look something like WAKEUP:. Multiple wakeup messages are supported", + "parameters": { + "type": "object", + "properties": { + "seconds": {"type": "number", "description": "number of seconds in the future that the timer will wake you up"}, + "message": {"type": "string", "description": "wakeup message that will be sent to you"} + }, + "required": ["seconds", "message"] + } + } +} diff --git a/MAVProxy/modules/mavproxy_chat/chat_openai.py b/MAVProxy/modules/mavproxy_chat/chat_openai.py index 249d9eafb0..fb2b735c96 100644 --- a/MAVProxy/modules/mavproxy_chat/chat_openai.py +++ b/MAVProxy/modules/mavproxy_chat/chat_openai.py @@ -10,6 +10,7 @@ from pymavlink import mavutil import time, re from datetime import datetime +from threading import Thread, Lock import json import math @@ -30,6 +31,14 @@ def __init__(self, mpstate, status_cb=None, wait_for_command_ack_fn=None): # keep reference to wait_for_command_ack_fn self.wait_for_command_ack_fn = wait_for_command_ack_fn + # lock to prevent multiple threads sending text to the assistant at the same time + self.send_lock = Lock() + + # wakeup timer array + self.wakeup_schedule = [] + self.thread = Thread(target=self.check_wakeup_timers) + self.thread.start() + # initialise OpenAI connection self.client = None self.assistant = None @@ -87,71 +96,74 @@ def set_api_key(self, api_key_str): # send text to assistant def send_to_assistant(self, text): - # check connection - if not self.check_connection(): - return "chat: failed to connect to OpenAI" - - # create a new message - input_message = self.client.beta.threads.messages.create( - thread_id=self.assistant_thread.id, - role="user", - content=text - ) - if input_message is None: - return "chat: failed to create input message" - - # create a run - self.run = self.client.beta.threads.runs.create( - thread_id=self.assistant_thread.id, - assistant_id=self.assistant.id - ) - if self.run is None: - return "chat: failed to create run" - - # wait for run to complete - run_done = False - while not run_done: - # wait for one second - time.sleep(0.1) + # get lock + with self.send_lock: - # retrieve the run - latest_run = self.client.beta.threads.runs.retrieve( + # check connection + if not self.check_connection(): + return "chat: failed to connect to OpenAI" + + # create a new message + input_message = self.client.beta.threads.messages.create( thread_id=self.assistant_thread.id, - run_id=self.run.id + role="user", + content=text ) + if input_message is None: + return "chat: failed to create input message" - # check run status - if latest_run.status in ["queued", "in_progress", "cancelling"]: - run_done = False - elif latest_run.status in ["cancelled", "failed", "completed", "expired"]: - run_done = True - elif latest_run.status in ["requires_action"]: - self.handle_function_call(latest_run) - run_done = False - else: - print("chat: unrecognised run status" + latest_run.status) - run_done = True - - # send status to status callback - self.send_status(latest_run.status) - - # retrieve messages on the thread - reply_messages = self.client.beta.threads.messages.list(self.assistant_thread.id, order = "asc", after=input_message.id) - if reply_messages is None: - return "chat: failed to retrieve messages" - - # concatenate all messages into a single reply skipping the first which is our question - reply = "" - need_newline = False - for message in reply_messages.data: - reply = reply + message.content[0].text.value - if need_newline: - reply = reply + "\n" - need_newline = True - - if reply is None or reply == "": - return "chat: failed to retrieve latest reply" - return reply + # create a run + self.run = self.client.beta.threads.runs.create( + thread_id=self.assistant_thread.id, + assistant_id=self.assistant.id + ) + if self.run is None: + return "chat: failed to create run" + + # wait for run to complete + run_done = False + while not run_done: + # wait for one second + time.sleep(0.1) + + # retrieve the run + latest_run = self.client.beta.threads.runs.retrieve( + thread_id=self.assistant_thread.id, + run_id=self.run.id + ) + + # check run status + if latest_run.status in ["queued", "in_progress", "cancelling"]: + run_done = False + elif latest_run.status in ["cancelled", "failed", "completed", "expired"]: + run_done = True + elif latest_run.status in ["requires_action"]: + self.handle_function_call(latest_run) + run_done = False + else: + print("chat: unrecognised run status" + latest_run.status) + run_done = True + + # send status to status callback + self.send_status(latest_run.status) + + # retrieve messages on the thread + reply_messages = self.client.beta.threads.messages.list(self.assistant_thread.id, order = "asc", after=input_message.id) + if reply_messages is None: + return "chat: failed to retrieve messages" + + # concatenate all messages into a single reply skipping the first which is our question + reply = "" + need_newline = False + for message in reply_messages.data: + reply = reply + message.content[0].text.value + if need_newline: + reply = reply + "\n" + need_newline = True + + if reply is None or reply == "": + return "chat: failed to retrieve latest reply" + return reply # handle function call request from assistant # on success this returns the text response that should be sent to the assistant, returns None on failure @@ -183,6 +195,16 @@ def handle_function_call(self, run): recognised_function = True output = json.dumps(self.get_vehicle_type()) + # get mode mapping + if tool_call.function.name == "get_mode_mapping": + recognised_function = True + try: + arguments = json.loads(tool_call.function.arguments) + output = self.get_mode_mapping(arguments) + except: + output = tool_call.function.name + ": failed" + print("chat: " + output) + # get vehicle state including armed, mode if tool_call.function.name == "get_vehicle_state": recognised_function = True @@ -271,6 +293,36 @@ def handle_function_call(self, run): output = "set_parameter: failed to set parameter value" print("chat: set_parameter: failed to set parameter value") + # set a wakeup timer + if tool_call.function.name == "set_wakeup_timer": + recognised_function = True + try: + arguments = json.loads(tool_call.function.arguments) + output = self.set_wakeup_timer(arguments) + except: + output = tool_call.function.name + ": failed" + print("chat: " + output) + + # get wakeup timers + if tool_call.function.name == "get_wakeup_timers": + recognised_function = True + try: + arguments = json.loads(tool_call.function.arguments) + output = self.get_wakeup_timers(arguments) + except: + output = tool_call.function.name + ": failed" + print("chat: " + output) + + # delete wakeup timers + if tool_call.function.name == "delete_wakeup_timers": + recognised_function = True + try: + arguments = json.loads(tool_call.function.arguments) + output = self.delete_wakeup_timers(arguments) + except: + output = tool_call.function.name + ": failed" + print("chat: " + output) + if not recognised_function: print("chat: handle_function_call: unrecognised function call: " + tool_call.function.name) output = "unrecognised function call: " + tool_call.function.name @@ -327,6 +379,49 @@ def get_vehicle_type(self): "vehicle_type": vehicle_type_str } + # get mode mapping + def get_mode_mapping(self, arguments): + # get name and/or number arguments + mode_name = arguments.get("name", None) + if mode_name is not None: + mode_name = mode_name.upper() + mode_number = arguments.get("number", None) + if mode_number is not None: + mode_number = int(mode_number) + + # check mode mapping is available + if self.mpstate.master() is None or self.mpstate.master().mode_mapping() is None: + return "get_mode_mapping: failed to retrieve mode mapping" + + # prepare list of modes + mode_list = [] + mode_mapping = self.mpstate.master().mode_mapping() + + # handle request for all modes + if mode_name is None and mode_number is None: + for mname in mode_mapping: + mnumber = mode_mapping[mname] + mode_list.append({"name": mname.upper(), "number": mnumber}) + + # handle request using mode name + elif mode_name is not None: + for mname in mode_mapping: + if mname.upper() == mode_name: + mode_list.append({"name": mname.upper(), "number": mode_mapping[mname]}) + + # handle request using mode number + elif mode_number is not None: + for mname in mode_mapping: + mnumber = mode_mapping[mname] + if mnumber == mode_number: + mode_list.append({"name": mname.upper(), "number": mnumber}) + + # return list of modes + try: + return json.dumps(mode_list) + except: + return "get_mode_mapping: failed to convert mode list to json" + # get vehicle state including armed, mode def get_vehicle_state(self): # get mode from latest HEARTBEAT message @@ -538,6 +633,105 @@ def set_parameter(self, arguments): self.mpstate.functions.param_set(param_name, param_value, retries=3) return "set_parameter: parameter value set" + # set a wakeup timer + def set_wakeup_timer(self, arguments): + # check required arguments are specified + seconds = arguments.get("seconds", -1) + if seconds < 0: + return "set_wakeup_timer: seconds not specified" + message = arguments.get("message", None) + if message is None: + return "set_wakeup_timer: message not specified" + + # add timer to wakeup schedule + self.wakeup_schedule.append({"time": time.time() + seconds, "message": message}) + return "set_wakeup_timer: wakeup timer set" + + # get wake timers + def get_wakeup_timers(self, arguments): + # check message argument, default to None meaning all + message = arguments.get("message", None) + + # prepare list of matching timers + matching_timers = [] + + # handle simple case of all timers + if message is None: + matching_timers = self.wakeup_schedule + + # handle regex in message + elif self.contains_regex(message): + message_pattern = re.compile(message, re.IGNORECASE) + for wakeup_timer in self.wakeup_schedule: + if message_pattern.match(wakeup_timer["message"]) is not None: + matching_timers.append(wakeup_timer) + + # handle case of a specific message + else: + for wakeup_timer in self.wakeup_schedule: + if wakeup_timer["message"] == message: + matching_timers.append(wakeup_timer) + + # return matching timers + try: + return json.dumps(matching_timers) + except: + return "get_wakeup_timers: failed to convert wakeup timer list to json" + + # delete wake timers + def delete_wakeup_timers(self, arguments): + # check message argument, default to all + message = arguments.get("message", None) + + # find matching timers + num_timers_deleted = 0 + + # handle simple case of deleting all timers + if message is None: + num_timers_deleted = len(self.wakeup_schedule) + self.wakeup_schedule.clear() + + # handle regex in message + elif self.contains_regex(message): + message_pattern = re.compile(message, re.IGNORECASE) + for wakeup_timer in self.wakeup_schedule: + if message_pattern.match(wakeup_timer["message"]) is not None: + num_timers_deleted = num_timers_deleted + 1 + self.wakeup_schedule.remove(wakeup_timer) + else: + # handle simple case of a single message + for wakeup_timer in self.wakeup_schedule: + if wakeup_timer["message"] == message: + num_timers_deleted = num_timers_deleted + 1 + self.wakeup_schedule.remove(wakeup_timer) + + # return number deleted and remaining + return "delete_wakeup_timers: deleted " + str(num_timers_deleted) + " timers, " + str(len(self.wakeup_schedule)) + " remaining" + + # check if any wakeup timers have expired and send messages if they have + # this function never returns so it should be called from a new thread + def check_wakeup_timers(self): + while True: + # wait for one second + time.sleep(1) + + # check if any timers are set + if len(self.wakeup_schedule) == 0: + continue + + # get current time + now = time.time() + + # check if any timers have expired + for wakeup_timer in self.wakeup_schedule: + if now >= wakeup_timer["time"]: + # send message to assistant + message = "WAKEUP:" + wakeup_timer["message"] + self.send_to_assistant(message) + + # remove from wakeup schedule + self.wakeup_schedule.remove(wakeup_timer) + # wrap latitude to range -90 to 90 def wrap_latitude(self, latitude_deg): if latitude_deg > 90: diff --git a/MAVProxy/modules/mavproxy_chat/chat_window.py b/MAVProxy/modules/mavproxy_chat/chat_window.py index ad25c9116c..df5e790f45 100644 --- a/MAVProxy/modules/mavproxy_chat/chat_window.py +++ b/MAVProxy/modules/mavproxy_chat/chat_window.py @@ -7,16 +7,13 @@ from MAVProxy.modules.lib.wx_loader import wx from MAVProxy.modules.mavproxy_chat import chat_openai, chat_voice_to_text -from threading import Thread, Lock +from threading import Thread class chat_window(): def __init__(self, mpstate, wait_for_command_ack_fn): # keep reference to mpstate self.mpstate = mpstate - # lock to prevent multiple threads sending text to the assistant at the same time - self.send_lock = Lock() - # create chat_openai object self.chat_openai = chat_openai.chat_openai(self.mpstate, self.set_status_text, wait_for_command_ack_fn) @@ -150,32 +147,30 @@ def text_input_change(self, event): # send text to assistant. should be called from a separate thread to avoid blocking def send_text_to_assistant(self): - # get lock - with self.send_lock: - # disable buttons and text input to stop multiple inputs (can't be done from a thread or must use CallAfter) - wx.CallAfter(self.record_button.Disable) - wx.CallAfter(self.text_input.Disable) - wx.CallAfter(self.send_button.Disable) - - # get text from text input and clear text input - send_text = self.text_input.GetValue() - wx.CallAfter(self.text_input.Clear) - - # copy user input text to reply box - orig_text_attr = self.text_reply.GetDefaultStyle() - wx.CallAfter(self.text_reply.SetDefaultStyle, wx.TextAttr(wx.RED)) - wx.CallAfter(self.text_reply.AppendText, send_text + "\n") - - # send text to assistant and place reply in reply box - reply = self.chat_openai.send_to_assistant(send_text) - if reply: - wx.CallAfter(self.text_reply.SetDefaultStyle, orig_text_attr) - wx.CallAfter(self.text_reply.AppendText, reply + "\n\n") - - # reenable buttons and text input (can't be done from a thread or must use CallAfter) - wx.CallAfter(self.record_button.Enable) - wx.CallAfter(self.text_input.Enable) - wx.CallAfter(self.send_button.Enable) + # disable buttons and text input to stop multiple inputs (can't be done from a thread or must use CallAfter) + wx.CallAfter(self.record_button.Disable) + wx.CallAfter(self.text_input.Disable) + wx.CallAfter(self.send_button.Disable) + + # get text from text input and clear text input + send_text = self.text_input.GetValue() + wx.CallAfter(self.text_input.Clear) + + # copy user input text to reply box + orig_text_attr = self.text_reply.GetDefaultStyle() + wx.CallAfter(self.text_reply.SetDefaultStyle, wx.TextAttr(wx.RED)) + wx.CallAfter(self.text_reply.AppendText, send_text + "\n") + + # send text to assistant and place reply in reply box + reply = self.chat_openai.send_to_assistant(send_text) + if reply: + wx.CallAfter(self.text_reply.SetDefaultStyle, orig_text_attr) + wx.CallAfter(self.text_reply.AppendText, reply + "\n\n") + + # reenable buttons and text input (can't be done from a thread or must use CallAfter) + wx.CallAfter(self.record_button.Enable) + wx.CallAfter(self.text_input.Enable) + wx.CallAfter(self.send_button.Enable) # set status text def set_status_text(self, text):