# Project export: Spot my good boy

This document was generated by HackStack to give an AI agent context about a hackathon project. Sections are labeled with their provenance; content marked as truncated was cut to keep this document small.

## Project metadata

- Hackathon: TreeHacks 2024
- Tagline: Introducing Spot the good boy: your memory's best friend! 🐾 Never forget a thing with Spot ⭐. Life-changing companionship for seniors🧓!
- Devpost: https://devpost.com/software/spot-my-good-boy
- GitHub: http://github.com/JosselinSomervilleRoberts/hackathon-spot
- Demo: https://drive.google.com/file/d/1tcM-7qPvPwKg9kgmvq4lUHC6OBiNflUE/view
- Video: https://player.vimeo.com/video/914183218?byline=0&portrait=0&title=0#t=
- Team: 4 GitHub contributor(s) — JosselinSomervilleRoberts (36 commits), yonigozlan (5 commits), Joachim Studnia (1 commits), avakn4 (1 commits)

## Devpost submission (written by the team)

### Inspiration

💡 Our inspiration stemmed from a desire to empower 💪 elderly individuals who may struggle with memory loss to live more independently and confidently in their own homes. We recognized the importance of creating a solution that not only addresses the practical challenge of finding objects but also provides companionship and reassurance. 😊

### What it does

🤖 Spot my good boy is a revolutionary dog-like 🐾 robot designed to assist the elderly in locating misplaced items within their living space. Using advanced technology and cutting-edge AI, it responds to natural language commands, navigates the environment to find requested objects, and provides verbal feedback to confirm its success😊🎉. Additionally, it serves as a vigilant companion, capable of alerting users to potential hazards in their surroundings. 🤖🔍👀

### How we built it

🛠️ We leveraged cutting-edge technology to retrofit a Boston Dynamics robot with custom software and hardware components. Through intensive programming and integration efforts, we enabled the robot to understand and execute complex commands, navigate autonomously, and communicate effectively with users. 💻🌟 This involved extensive collaboration between a huge diversity of technical experts 👏👥 from our team: software engineers, hardware specialists, and user experience designers. 💻🛠️👩‍💻

### Challenges we ran into

😵 Developing Spot my good boy presented several significant challenges, including: 😨 Integrating natural language processing capabilities to ensure seamless communication. 🤖🗣️ Designing an intuitive user interface for elderly users with varying levels of technological familiarity. 👵👴🖥️ Implementing robust object recognition and navigation algorithms to locate items accurately. 📸🔎 Managing multiple AI agents and API calls to services such as Google Text-to-Speech (gTTS), OpenAI's Whispr, GPT-4, and OpenCV cascade models for object detection. 🤖🤖🤖 Hardware integration for sound, microphone, camera, and movement controls, ensuring seamless interaction and functionality across different components. 🎤📷🕹️

### Accomplishments we're proud of

🎉 We are immensely proud of several accomplishments, including: Successfully creating a functional prototype of Spot my good boy within the constraints of the hackathon timeframe. ⏱️🤖 Demonstrating the robot's ability to understand and respond to verbal commands in real-time. 🎙️🤖 Implementing reliable object detection and navigation algorithms, ensuring accurate retrieval of items. 📸🧭🔍 Establishing a foundation for future development and refinement of the technology. 🚀🌟

### What we learned

💡 Our journey with Spot my good boy taught us invaluable lessons about teamwork, innovation, and the power of technology to positively impact lives. 🌟🚀 Key takeaways include: The importance of user-centered design in creating inclusive and accessible solutions. 👥🔎 The challenges and complexities of integrating hardware and software components in robotics projects. 🤖🔨 The significance of empathy and understanding when designing for vulnerable populations. 👵👴💞 The potential for technology to enhance the quality of life for elderly individuals and caregivers alike. 🌟📈

### What's next

🚀 Moving forward, we envision several exciting opportunities to further enhance and refine Spot my good boy: Conducting additional user testing and feedback sessions to iteratively improve the robot's functionality and user experience 👥📝 Exploring potential partnerships with assisted living facilities and healthcare providers to deploy Spot in real-world environments 🏥👵 Continuously updating and expanding Spot's capabilities through software updates 📲🔄 Investigating advanced features such as fall detection, medication reminders, and remote monitoring capabilities 👀💊⏰ Collaborating with researchers and industry experts to explore the broader implications of robotics in eldercare and aging-in-place initiatives 👴👵🤖 With dedication and innovation, Spot my good boy has the potential to revolutionize the way we support and care for elderly individuals, empowering them to live independently and confidently in their own homes. 🏡👵👍

## README (from the GitHub repository)

# Spot My Good Boy

This project is the result of the collaboration between Yoni, Ava, Josselin,
and Joachim for Stanford TreeHacks 2024 (February 16 - 18, 2024).

## Short description
Introducing Spot My Good Boy: your memory's best friend! Never forget a thing with
Spot. Life-changing companionship for seniors!

## Inspiration
Our inspiration stemmed from a desire to empower 💪 elderly individuals who may struggle with memory loss to live more independently and confidently in their own homes. We recognized the importance of creating a solution that not only addresses the practical challenge of finding objects but also provides companionship and reassurance. 😊

## What it does
Spot my good boy is a revolutionary dog-like 🐾 robot designed to assist the elderly in locating misplaced items within their living space. Using advanced technology and cutting-edge AI, it responds to natural language commands, navigates the environment to find requested objects, and provides verbal feedback to confirm its success😊🎉. Additionally, it serves as a vigilant companion, capable of alerting users to potential hazards in their surroundings. 🤖🔍👀

## How we built it
We leveraged cutting-edge technology to retrofit a Boston Dynamics robot with custom software and hardware components. Through intensive programming and integration efforts, we enabled the robot to understand and execute complex commands, navigate autonomously, and communicate effectively with users. 💻🌟 This involved extensive collaboration between a huge diversity of technical experts 👏👥 from our team: software engineers, hardware specialists, and user experience designers. 💻🛠️👩‍💻

## Challenges we ran into
Developing Spot my good boy presented several significant challenges, including: 😨

- Integrating natural language processing capabilities to ensure seamless communication. 🤖🗣️
- Designing an intuitive user interface for elderly users with varying levels of technological familiarity. 👵👴🖥️
- Implementing robust object recognition and navigation algorithms to locate items accurately. 📸🔎
- Managing multiple AI agents and API calls to services such as Google Text-to-Speech (gTTS), OpenAI's Whispr, GPT-4, and OpenCV cascade models for object detection. 🤖🤖🤖
- Hardware integration for sound, microphone, camera, and movement controls, ensuring seamless interaction and functionality across different components. 🎤📷🕹️

## Accomplishments that we're proud of 🎉
We are immensely proud of several accomplishments, including:

- Successfully creating a functional prototype of Spot my good boy within the constraints of the hackathon timeframe. ⏱️🤖
- Demonstrating the robot's ability to understand and respond to verbal commands in real-time. 🎙️🤖
- Implementing reliable object detection and navigation algorithms, ensuring accurate retrieval of items. 📸🧭🔍
- Establishing a foundation for future development and refinement of the technology. 🚀🌟

## What we learned 💡
Our journey with Spot my good boy taught us invaluable lessons about teamwork, innovation, and the power of technology to positively impact lives. 🌟🚀 Key takeaways include:

- The importance of user-centered design in creating inclusive and accessible solutions. 👥🔎
- The challenges and complexities of integrating hardware and software components in robotics projects. 🤖🔨
- The significance of empathy and understanding when designing for vulnerable populations. 👵👴💞
- The potential for technology to enhance the quality of life for elderly individuals and caregivers alike. 🌟📈

## What's next for Spot my good boy 🚀
Moving forward, we envision several exciting opportunities to further enhance and refine Spot my good boy:

- Conducting additional user testing and feedback sessions to iteratively improve the robot's functionality and user experience 👥📝
- Exploring potential partnerships with assisted living facilities and healthcare providers to deploy Spot in real-world environments 🏥👵
- Continuously updating and expanding Spot's capabilities through software updates 📲🔄
- Investigating advanced features such as fall detection, medication reminders, and remote monitoring capabilities 👀💊⏰
- Collaborating with researchers and industry experts to explore the broader implications of robotics in eldercare and aging-in-place initiatives 👴👵🤖

With dedication and innovation, Spot my good boy has the potential to revolutionize the way we support and care for elderly individuals, empowering them to live independently and confidently in their own homes. 🏡👵👍

## Links
- Devpost: https://devpost.com/software/spot-my-good-boy
- Demo: https://vimeo.com/914183218

## Detected evidence (automated analysis)

Indexed codebase: 9 recognized source files, 32 KB.
- OpenAI (technology) — detected in the code
- Python (language) — detected in the code
- Docker (technology) — claimed on Devpost, not found in the code

## Codebase structure (from repository index)

### Files (13 of 13)

```
.gitignore
client.py
constants.py
Dockerfile
extract_class_answer.py
gpt4v.py
main.py
openai_client.py
README.md
requirements-dev.txt
requirements.txt
spot_controller.py
together_client.py
```

### Dependencies

- requirements.txt: gTTS@~=2.5.1, openai@~=1.12.0

### Recent commits (newest first)

- Merge pull request #1 from joa-stdn/master
- update spot'
- Last commit
- Add Mixtral
- Add Together client
- Final changes
- Final changes
- Fix negative rotation
- Changing rotation
- Compressing image
- Changes
- GPT4-V works and requirements
- Merge branch 'master' of https://github.com/JosselinSomervilleRoberts/hackathon-spot
- add gpt4v
- Whisper API
- update requirements
- Cleanup
- Update requirements
- Move speech
- Changing while loop

## Key source files (fetched from GitHub, selected and truncated for size)

### requirements.txt

```
gTTS~=2.5.1
openai~=1.12.0
```

### Dockerfile

```
FROM ghcr.io/merklebot/hackathon-arm-image:master as build

ENV PYTHONDONTWRITEBYTECODE 1
ENV PYTHONUNBUFFERED 1

ARG TARGETPLATFORM
ARG BUILDPLATFORM
ARG TARGETOS
ARG TARGETARCH

ARG Version
ARG GitCommit
RUN echo "I am running on $BUILDPLATFORM, building for $TARGETPLATFORM" 


COPY requirements.txt requirements.txt
RUN python3.8 -m pip install -r requirements.txt
COPY . .

CMD ["python3.8", "main.py"]

```

### main.py

```python
import os
import time
from extract_class_answer import process_question_attempts
from openai_client import OpenAIClient, speech_to_text, find_object_in_image
from together_client import TogetherClient

from constants import OBJ_CLASSES

# Attempt to import SpotController, set flag if not available
try:
    from spot_controller import SpotController

    local_laptop = False
except ImportError:
    local_laptop = True
print(f"Local laptop: {local_laptop}")
from gtts import gTTS
import cv2
from typing import Callable, Any

ROBOT_IP = "10.0.0.3"  # os.environ['ROBOT_IP']
SPOT_USERNAME = "admin"  # os.environ['SPOT_USERNAME']
SPOT_PASSWORD = "2zqa8dgw7lor"  # os.environ['SPOT_PASSWORD']


# Wrapper class
class SpotControllerWrapper:
    def __init__(self, *args, **kwargs):
        if not local_laptop:
            self.spot = SpotController(*args, **kwargs)

    def __getattr__(self, name):
        """If local_laptop is True, replace SpotController methods with no-op.
        Otherwise, return method from SpotController."""
        if local_laptop:

            def method(*args, **kwargs):
                print(f"Skipping {name} due to local execution.")

            return method
        else:
            return getattr(self.spot, name)

    def __enter__(self):
        if not local_laptop:
            return self.spot.__enter__()
        return self  # Return self to work with context manager syntax

    def __exit__(self, exc_type, exc_value, traceback):
        if not local_laptop:
            return self.spot.__exit__(exc_type, exc_value, traceback)


if local_laptop:
    SpotClass = SpotControllerWrapper
else:
    SpotClass = SpotController

ROBOT_IP = "10.0.0.3"  # os.environ['ROBOT_IP']
SPOT_USERNAME = "admin"  # os.environ['SPOT_USERNAME']
SPOT_PASSWORD = "2zqa8dgw7lor"  # os.environ['SPOT_PASSWORD']


def say_something(text: str, file_name: str = "welcome.mp3"):
    print(f"Say something")
    print(f"\t- Saying: {text}")
    myobj = gTTS(text=text, lang="en", slow=False)
    myobj.save(file_name)
    # Play loud audio
    # Amplify audio
    os.system(f"ffmpeg -i {file_name} -filter:a 'volume=2.0' temp_{file_name} -y")
    # Play amplified audio
    os.system(f"ffplay -nodisp -autoexit -loglevel quiet temp_{file_name}")
    print(f"\t- Done saying something")


def nod_head(x: int, spot: SpotControllerWrapper):
    print(f"Nodding head {x} times")
    # Nod head x times
    for _ in range(x):
        print(f"\t- Moving head up")
        spot.move_head_in_points(
            yaws=[0, 0], pitches=[0.18, 0], rolls=[0, 0], sleep_after_point_reached=0
        )
        print(f"\t- Moving head down")
        spot.move_head_in_points(
            yaws=[0, 0], pitches=[-0.1, 0], rolls=[0, 0], sleep_after_point_reached=0
        )
    # Reset head position
    print(f"\t- Resetting head position")
    spot.move_head_in_points(
        yaws=[0, 0], pitches=[0, 0], rolls=[0, 0], sleep_after_point_reached=0
    )
    print(f"\t- Done nodding head")


def detect_object(
    spot: SpotControllerWrapper,
    camera_capture: cv2.VideoCapture,
    obj_class: str,
):
    for _ in range(10):
        frame = camera_capture.read()[1]
    return 1 if find_object_in_image(frame, obj_class) else 0


def rotate_and_run_function(
    spot: SpotControllerWrapper,
    function: Callable[[SpotControllerWrapper, Any], int],
    every_n_milliseconds: int,
    rotation_speed: float,
    n_rotations: int,
    **kwargs,
) -> bool:
    """Rotate the robot for n_rotations and run the function every_n_milliseconds

    Args:
        spot (SpotController): SpotController object
        function (Callable[[SpotController, Any], int]): Function to run
            This function should return 1 if the robot should stop
        every_n_milliseconds (int): Run function every n milliseconds
        rotation_speed (float): Rotation speed in rad/s
        n_rotations (int): Number of rotations

    Returns:
        int: The result of the function
    """
    duration: int = n_rotations * 2 * 3.14 / abs(rotation_speed)
    print(f"Rotate and run function")
    print(f"\t- Rotating for {n_rotations} rotations during {duration} seconds")
    print(f"\t- Going to execute function every {every_n_milliseconds} milliseconds")
    result: int = 0
    start_time = time.time()
    last_command_time_ms = start_time * 1000 - every_n_milliseconds
    delay = 0
    while time.time() - start_time < duration:
        spot.move_by_velocity_control(
            v_x=0,
            v_y=0,
            v_rot=rotation_speed,
            cmd_duration=2,
        )
        start_exec_time = time.time()
        if (time.time() * 1000 - last_command_time_ms) >= every_n_milliseconds:
            last_command_time_ms = time.time() * 1000
            result: int = function(spot, **kwargs)
            if result == 1:
                print("\t- Function returned 1, stopping")
                delay = time.time() - start_exec_time
                break
    print("\t- Stopping")
    spot.move_by_velocity_control(
        v_x=0,
        v_y=0,
        v_rot=0,
        cmd_duration=0.1,
    )
    print("\t- Done rotating and running function")
    return result == 1, delay


def record_audio(sample_name: str = "recording.wav", duration: int = 7) -> str:
    print("Recording audio")
    if local_laptop:
        cmd = (
            f"arecord -vv --format=cd -r 48000 --duration={duration} -c 1 {sample_name}"
        )
    else:
        cmd = f'arecord -vv --format=cd --device={os.environ["AUDIO_INPUT_DEVICE"]} -r 48000 --duration={duration} -c 1 {sample_name}'
    print(f"\t- Running command: {cmd}")
    os.system(cmd)
    print(f"\t- Done recording audio")
    result = speech_to_text(sample_name)
    print(f"\t- Transcribed audio: {result}")
    return result


def main():
    # Capture image
    camera_capture = cv2.VideoCapture(0)

    say_something("Booting up the robot")
    # Load the Haar Cascade for face detection
    face_cascade = cv2.CascadeClassifier(
    
[truncated — 3678 more characters]
```

### constants.py

```python
OBJ_CLASSES = [
    "person",
    "cup",
    "mouse",
    "remote",
    "cell phone",
    "book",
    "toothbrush",
]

```

### client.py

```python
from abc import abstractmethod, ABC


class Client(ABC):

    def __init__(self, model_name: str, api_key: str):
        self.model_name = model_name
        self.api_key = api_key

    @abstractmethod
    def make_request(self, prompt: str):
        pass

```

### gpt4v.py

```python
import base64
import requests
import os
import cv2

# OpenAI API Key
api_key = os.environ.get("OPENAI_API_KEY")


# Path to your image
# Capture webcam image
camera_capture = cv2.VideoCapture(0)

counter = 0
while counter < 300:
    _, image = camera_capture.read()
    cv2.imshow("Webcam Object Detection", image)

    # Print text response

    if counter % 2 == 0:
        # Show image
        base64_image = base64.b64encode(cv2.imencode(".jpg", image)[1]).decode("utf-8")
        headers = {
            "Content-Type": "application/json",
            "Authorization": f"Bearer {api_key}",
        }

        payload = {
            "model": "gpt-4-vision-preview",
            "messages": [
                {
                    "role": "user",
                    "content": [
                        {
                            "type": "text",
                            "text": "Is there a cup in this image? Answer by a singler word: yes or no only.",
                        },
                        {
                            "type": "image_url",
                            "image_url": {
                                "url": f"data:image/jpeg;base64,{base64_image}"
                            },
                        },
                    ],
                }
            ],
            "max_tokens": 3,
        }

        response = requests.post(
            "https://api.openai.com/v1/chat/completions", headers=headers, json=payload
        )
        rep = response.json()["choices"][0]["message"]["content"]
        if "yes" in rep.lower():
            print("Yes, there is a cup in the image")
            break
        print(rep)
    counter += 1

    if cv2.waitKey(1) & 0xFF == ord("q"):
        break

camera_capture.release()
cv2.destroyAllWindows()

```

### together_client.py

```python
from typing import Dict
from client import Client

import requests


class TogetherClientError(Exception):
    pass


class TogetherClient(Client):
    """
    Client for the models where we evaluate offline. Since the queries are handled offline, the `TogetherClient` just
    checks if the request/result is cached. We return the result if it's in the cache. Otherwise, we return an error.
    """

    INFERENCE_ENDPOINT: str = "https://api.together.xyz/api/inference"

    def __init__(self, model_name: str, api_key: str):
        self.api_key: str = api_key
        self.model_name = model_name

    def _get_job_url(self, job_id: str) -> str:
        return f"https://api.together.xyz/jobs/job/{job_id}"

    def make_request(self, prompt: str):
        raw_request = {
            "request_type": "language-model-inference",
            "model": self.model_name,
            "prompt": prompt,
        }

        if not self.api_key:
            raise TogetherClientError("togetherApiKey not set in credentials.conf")
        headers: Dict[str, str] = {"Authorization": f"Bearer {self.api_key}"}

        try:
            response = requests.post(
                TogetherClient.INFERENCE_ENDPOINT, headers=headers, json=raw_request
            )
            try:
                response.raise_for_status()
            except Exception as e:
                raise TogetherClientError(
                    f"Together request failed with {response.status_code}: {response.text}"
                ) from e
            result = response.json()
            if "output" not in result:
                raise TogetherClientError(
                    f"Could not get output from Together response: {result}"
                )
            if "error" in result["output"]:
                error_message = result["output"]["error"]
                raise TogetherClientError(
                    f"Together request failed with error: {error_message}"
                )
            return result["output"]["choices"][0]["text"], None

        except Exception as error:
            return "", error

```

### extract_class_answer.py

```python
from typing import List, Dict
from client import Client
import json

# Define your prompt
BASE_PROMPT = (
    "An elderly user will ask you a question, and you should answer"
    + "in a useful and harmless way in a very precise JSON format. Additionally, if"
    + "the question involves finding an object, please answer first in a cordial"
    + "fashion that you will try to help find it and then return the class"
    + "to which the object belongs, in this order.\n"
    + "The class has to belong to the following set: "
)

EXAMPLES = (
    "If there is no object to find or if the object to find does not "
    + "belong to the previous set, return an empty object_class_to_find, ie ''. "
    + "\nHere are two examples.\n"
    + "User: What does the word 'hackathon' mean?\n "
    + "Assistant: "
    + "{'answer': 'A hackathon is an event, typically lasting "
    + "several days, where people, often including programmers, designers, "
    + "and others with various technical backgrounds, collaborate intensively "
    + "on software projects.', "
    + "'object_class_to_find': ''}\n"
    + "User: Where can I find my tea? \n "
    + "Assistant: {'answer': 'Sure, let me find your tea. Wait a second.', "
    + "'object_class_to_find': 'cup'}\n"
    + "Here in the user question.\n"
)

DEFAULT_DICT_OUTPUT = {
    "answer": "I am sorry, but I did not understand your question...",
    "object_class_to_find": "",
}


def create_prompt(obj_classes, question):
    prompt = BASE_PROMPT + "[" + ",".join(obj_classes) + "]" + EXAMPLES + question
    return prompt


def process_question(
    obj_classes: List[str], question: str, client: Client
) -> Dict[str, str]:
    prompt = create_prompt(obj_classes, question)
    output, error = client.make_request(prompt)
    if error is not None:
        raise Exception(f"Error processing question: {error}")
    dict_output = json.loads(output)
    assert "answer" in dict_output.keys()
    assert "object_class_to_find" in dict_output.keys()
    obj_class = dict_output["object_class_to_find"]
    assert obj_class in [""] + obj_classes
    return dict_output


def process_question_attempts(obj_classes, question, client: Client, num_attempts=2):
    dict_output = DEFAULT_DICT_OUTPUT
    for _ in range(num_attempts):
        try:
            dict_output = process_question(obj_classes, question, client)
            break
        except Exception as e:
            print(f"Error processing question: {e}")
            continue

    return dict_output


if __name__ == "__main__":
    from constants import OBJ_CLASSES

    question = "Can you help me find my cup?"
    dict_output = process_question_attempts(OBJ_CLASSES, question, num_attempts=2)
    print(dict_output)

```

### openai_client.py

```python
from openai import OpenAI
import json
import os
import cv2
import base64
import requests
from client import Client

# Load API keys from JSON file

# Get OpenAI API key
openai_api_key = os.environ.get("OPENAI_API_KEY")

# Check if OpenAI API key exists
if not openai_api_key:
    raise ValueError("OpenAI API key not found in api_keys.json")

client = OpenAI(api_key=openai_api_key)


class OpenAIClient(Client):

    def make_request(self, prompt: str):
        response = client.chat.completions.create(
            model=self.model_name,  # "gpt-4-1106-preview"
            response_format={"type": "json_object"},
            messages=[
                {"role": "system", "content": "You are a helpful assistant."},
                {
                    "role": "user",
                    "content": prompt,
                },
            ],
        )
        output = response.choices[0].message.content
        return output, None


def speech_to_text(file_name: str) -> str:
    """
    Transcribe an audio file to text using the Google Cloud Speech-to-Text API.
    Args:
        file_name (str): The name of the audio file to transcribe.
    Returns
        str: The transcribed text.
    """
    audio_file = open(file_name, "rb")
    transcript = client.audio.transcriptions.create(
        model="whisper-1", file=audio_file, language="en", response_format="text"
    )
    return transcript


def find_object_in_image(image, obj_class: str) -> bool:
    print("find_object_in_image")
    print(f"\t- Looking for a {obj_class} in the image...")
    # Encode it with compressing this time
    base64_image = base64.b64encode(
        cv2.imencode(".jpg", image, [cv2.IMWRITE_JPEG_QUALITY, 20])[1]
    ).decode("utf-8")
    print(f"\t- Sized of base64_image: {len(base64_image)}")

    headers = {
        "Content-Type": "application/json",
        "Authorization": f"Bearer {openai_api_key}",
    }

    payload = {
        "model": "gpt-4-vision-preview",
        "messages": [
            {
                "role": "user",
                "content": [
                    {
                        "type": "text",
                        "text": f"Is there a {obj_class} in this image? Answer by a singler word: yes or no only.",
                    },
                    {
                        "type": "image_url",
                        "image_url": {"url": f"data:image/jpeg;base64,{base64_image}"},
                    },
                ],
            }
        ],
        "max_tokens": 2,
    }

    response = requests.post(
        "https://api.openai.com/v1/chat/completions", headers=headers, json=payload
    )
    rep = response.json()["choices"][0]["message"]["content"]
    print(f"\t- GPT-4 Vision response: {rep}")
    return "yes" in rep.lower()

```

### spot_controller.py

```python
import time
import bosdyn.client
from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder, blocking_stand  # , blocking_sit
from bosdyn.geometry import EulerZXY
from bosdyn.api.spot import robot_command_pb2 as spot_command_pb2
from bosdyn.client.frame_helpers import ODOM_FRAME_NAME
from bosdyn.api.basic_command_pb2 import RobotCommandFeedbackStatus
from bosdyn.client.estop import EstopClient, EstopEndpoint, EstopKeepAlive
from bosdyn.client.robot_state import RobotStateClient
from bosdyn.client.frame_helpers import ODOM_FRAME_NAME, VISION_FRAME_NAME, BODY_FRAME_NAME, \
    GRAV_ALIGNED_BODY_FRAME_NAME, get_se2_a_tform_b
from bosdyn.client import math_helpers

import traceback

VELOCITY_CMD_DURATION = 0.5


class SpotController:
    def __init__(self, username, password, robot_ip):
        self.username = username
        self.password = password
        self.robot_ip = robot_ip

        sdk = bosdyn.client.create_standard_sdk('ControllingSDK')

        self.robot = sdk.create_robot(robot_ip)
        id_client = self.robot.ensure_client('robot-id')

        self.robot.authenticate(username, password)
        self.command_client = self.robot.ensure_client(RobotCommandClient.default_service_name)
        self.robot.logger.info("Authenticated")

        self._lease_client = None
        self._lease = None
        self._lease_keepalive = None

        self._estop_client = self.robot.ensure_client(EstopClient.default_service_name)
        self._estop_endpoint = EstopEndpoint(self._estop_client, 'GNClient', 9.0)
        self._estop_keepalive = None

        self.state_client = self.robot.ensure_client(RobotStateClient.default_service_name)

    def release_estop(self):
        self._estop_endpoint.force_simple_setup()
        self._estop_keepalive = EstopKeepAlive(self._estop_endpoint)

    def set_estop(self):
        if self._estop_keepalive:
            try:
                self._estop_keepalive.stop()
            except:
                self.robot.logger.error("Failed to set estop")
                traceback.print_exc()
            self._estop_keepalive.shutdown()
            self._estop_keepalive = None

    def lease_control(self):
        self._lease_client = self.robot.ensure_client('lease')
        self._lease = self._lease_client.take()
        self._lease_keepalive = bosdyn.client.lease.LeaseKeepAlive(self._lease_client, must_acquire=True)
        self.robot.logger.info("Lease acquired")

    def return_lease(self):
        self._lease_client.return_lease(self._lease)
        self._lease_keepalive.shutdown()
        self._lease_keepalive = None

    def __enter__(self):
        self.lease_control()
        self.release_estop()
        self.power_on_stand_up()
        return self

    def __exit__(self, exc_type, exc_val, exc_tb):
        if exc_type:
            self.robot.logger.error("Spot powered off with " + exc_val + " exception")
        self.power_off_sit_down()
        self.return_lease()
        self.set_estop()

        return True if exc_type else False

    def move_head_in_points(self, yaws, pitches, rolls, body_height=0, sleep_after_point_reached=0, timeout=3):
        for i in range(len(yaws)):
            footprint_r_body = EulerZXY(yaw=yaws[i], roll=rolls[i], pitch=pitches[i])
            params = RobotCommandBuilder.mobility_params(footprint_R_body=footprint_r_body, body_height=body_height)
            blocking_stand(self.command_client, timeout_sec=timeout, update_frequency=0.02, params=params)
            self.robot.logger.info("Moved to yaw={} rolls={} pitch={}".format(yaws[i], rolls[i], pitches[i]))
            if sleep_after_point_reached:
                time.sleep(sleep_after_point_reached)

    def wait_until_action_complete(self, cmd_id, timeout=15):
        start_time = time.time()
        while time.time() - start_time < timeout:
            feedback = self.command_client.robot_command_feedback(cmd_id)
            mobility_feedback = feedback.feedback.synchronized_feedback.mobility_command_feedback
            if mobility_feedback.status != RobotCommandFeedbackStatus.STATUS_PROCESSING:
                print("Failed to reach the goal")
                return False
            traj_feedback = mobility_feedback.se2_trajectory_feedback
            if (traj_feedback.status == traj_feedback.STATUS_AT_GOAL and
                    traj_feedback.body_movement_status == traj_feedback.BODY_STATUS_SETTLED):
                print("Arrived at the goal.")
                return True
            time.sleep(0.5)

    def move_to_goal(self, goal_x=0, goal_y=0):
        cmd = RobotCommandBuilder.synchro_trajectory_command_in_body_frame(
            goal_x_rt_body=goal_x,
            goal_y_rt_body=goal_y,
            goal_heading_rt_body=0,
            frame_tree_snapshot=self.robot.get_frame_tree_snapshot()
        )
        # cmd = RobotCommandBuilder.synchro_se2_trajectory_point_command(goal_x=goal_x, goal_y=goal_y, goal_heading=0,
        #                                                                frame_name=GRAV_ALIGNED_BODY_FRAME_NAME)
        cmd_id = self.command_client.robot_command(lease=None, command=cmd,
                                                   end_time_secs=time.time() + 10)
        self.wait_until_action_complete(cmd_id)

        self.robot.logger.info("Moved to x={} y={}".format(goal_x, goal_y))

    def power_on_stand_up(self):
        self.robot.power_on(timeout_sec=20)
        assert self.robot.is_powered_on(), "Not powered on"
        self.robot.time_sync.wait_for_sync()
        blocking_stand(self.command_client, timeout_sec=10)

    def power_off_sit_down(self):
        self.move_head_in_points(yaws=[0], pitches=[0], rolls=[0])
        self.robot.power_off(cut_immediately=False)

    def make_stance(self, x_offset, y_offset):
        state = self.state_client.get_robot_state()
        vo_T_body = get_se2_a_tform_b(state.kinematic_state.transforms_snapshot,
                                      VISION_FRAME_NAME,
   
[truncated — 2131 more characters]
```