robocrew 0.0.1__py3-none-any.whl

robocrew/core/LLMAgent.py

@@ -0,0 +1,131 @@
+from robocrew.core.utils import horizontal_angle_grid
+from robocrew.core.sound_receiver import SoundReceiver
+from dotenv import find_dotenv, load_dotenv
+import cv2
+import base64
+from langchain_core.tools import tool
+from langchain_core.messages import HumanMessage, SystemMessage, ToolMessage
+from langchain.chat_models import init_chat_model
+import queue
+load_dotenv(find_dotenv())
+
+
+class LLMAgent():
+    def __init__(self, model, tools, system_prompt=None, main_camera_usb_port=None, camera_fov=120, sounddevice_index=None, wakeword="robot", history_len=None):
+        """
+        model: name of the model to use
+        tools: list of langchain tools
+        system_prompt: custom system prompt - optional
+        main_camera_usb_port: provide usb port of your robot front camera if you want to use it.
+        camera_fov: field of view (degrees) of your main camera.
+        sounddevice_index: provide sounddevice index of your microphone if you want robot to hear.
+        wakeword: custom wakeword hearing which robot will set your sentence as a task o do.
+        history_len: if you want agent to have messages history cuttof, provide number of newest request-response pairs to keep.
+        """
+        base_system_prompt = "You are mobile robot with two arms."
+        self.task = "You are standing in a room. Explore the environment, find a backpack and approach it."
+        system_prompt = system_prompt or base_system_prompt
+        llm = init_chat_model(model)
+        self.llm = llm.bind_tools(tools, parallel_tool_calls=False)
+        self.tools = tools
+        self.system_message = SystemMessage(content=system_prompt)
+        self.message_history = [self.system_message]
+        # cameras
+        self.main_camera = cv2.VideoCapture(main_camera_usb_port) if main_camera_usb_port else None
+        self.hitory_len = history_len
+        if self.main_camera:
+            self.main_camera.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+            self.camera_fov = camera_fov
+        self.sounddevice_index = sounddevice_index
+        if self.sounddevice_index is not None:
+            self.task_queue = queue.Queue()
+            self.sound_receiver = SoundReceiver(sounddevice_index, self.task_queue, wakeword)
+            # self.task = ""
+
+    def capture_image(self):
+        self.main_camera.grab() # Clear the buffer
+        _, frame = self.main_camera.read()
+        frame = horizontal_angle_grid(frame, h_fov=self.camera_fov)
+        _, buffer = cv2.imencode('.jpg', frame)
+        return buffer.tobytes()
+
+    def invoke_tool(self, tool_call):
+        # convert string to real function
+        tool_name_to_tool = {tool.name: tool for tool in self.tools}
+        name = tool_call["name"]
+        requested_tool = tool_name_to_tool[name]
+        args = tool_call["args"]
+        tool_output = requested_tool.invoke(args)
+        return ToolMessage(tool_output, tool_call_id=tool_call["id"])
+    
+    def cut_off_context(self, nr_of_loops):
+        """
+        Trims the message history in the state to keep only the most recent context for the agent.
+        """        
+        ai_indices = [i for i, msg in enumerate(self.message_history) if msg.type == "human"]
+        if len(ai_indices) >= nr_of_loops:
+            start_index = ai_indices[-nr_of_loops]
+            self.message_history = [self.system_message] + self.message_history[start_index:]
+
+    def check_for_new_task(self):
+        """Non-blockingly checks the queue for a new task."""
+        if not self.task_queue.empty():
+            self.task = self.task_queue.get()
+
+
+    def go(self):
+        while True:
+            if self.main_camera:
+                image_bytes = self.capture_image()
+                image_base64 = base64.b64encode(image_bytes).decode('utf-8')
+                
+                message = HumanMessage(
+                    content=[
+                        {"type": "text", "text": "Here is the current view from your main camera. Use it to understand your current status."},
+                        {
+                            "type": "image_url",
+                            "image_url": {"url": f"data:image/jpeg;base64,{image_base64}"}
+                        },
+                        {"type": "text", "text": f"Your task is: '{self.task}'"}
+                    ]
+                )
+            else:
+                message = HumanMessage(content=f"Your task is: '{self.task}'")
+
+
+                
+            self.message_history.append(message)
+            response = self.llm.invoke(self.message_history)
+            print(response.content)
+            print(response.tool_calls)
+            self.message_history.append(response)
+            if self.hitory_len:
+                self.cut_off_context(self.hitory_len)
+
+            # execute tool
+            for tool_call in response.tool_calls:
+                tool_response = self.invoke_tool(tool_call)
+                self.message_history.append(tool_response)
+                if tool_call["name"] == "finish_task":
+                    return "Task finished, going idle."
+                
+            if self.sounddevice_index:
+                self.check_for_new_task()
+            
+
+if __name__ == "__main__":
+
+    @tool
+    def do_nothing() -> str:
+        """does nothing at all"""
+        print("Doing nothing...")
+        return "Doing nothing."
+    
+    agent = LLMAgent(
+        model="google_genai:gemini-robotics-er-1.5-preview",
+        tools=[
+            do_nothing,
+        ],
+    )
+    result = agent.go()
+    print(result)

robocrew/core/sound_receiver.py

@@ -0,0 +1,206 @@
+import pyaudio
+import wave
+import threading
+import time
+import audioop
+import time
+from openai import OpenAI
+import io
+from dotenv import find_dotenv, load_dotenv
+
+
+load_dotenv(find_dotenv())
+
+
+class SoundReceiver:
+    def __init__(self, sounddevice_index, task_queue=None, wakeword="robot"):
+        self.FORMAT = pyaudio.paInt16
+        self.CHANNELS = 1
+        self.RATE = 48000
+        self.BUFFER_SECONDS = 2
+        self.frames_per_buffer = 2048
+        self.recording_loop_delay = 0.2
+        # parse DEVICE_INDEX env var into an int if present, else None
+        self.DEVICE_INDEX = sounddevice_index
+        self.wakeword = wakeword
+
+        self._p = pyaudio.PyAudio()
+        self._sample_width = self._p.get_sample_size(self.FORMAT)
+        self._bytes_per_second = int(self.RATE * self.CHANNELS * self._sample_width)
+        self._buffer_capacity_bytes = int(self._bytes_per_second * self.BUFFER_SECONDS)
+        self.task_queue = task_queue
+
+        self._buffer = bytearray(self._buffer_capacity_bytes)
+        self._write_pos = 0
+        self._has_wrapped = False
+        self._lock = threading.RLock()
+
+        self._stream = None
+        self._listening = False
+        self._recording = False
+        self.RMS_THRESHOLD = 400.0
+        self.reciver_thread = threading.Thread(target=self._recorder_loop)
+        self.reciver_thread.daemon = True
+        self.recorded_frames = []
+        self.first_timestamp_below_threshold = None
+        self.num_recorded_buffers = 0
+        self.openai_client = OpenAI()
+        self.start_listening()
+
+
+    def _write_to_buffer(self, data: bytes):
+        with self._lock:
+            n = len(data)
+            if n == 0:
+                return
+            end_space = self._buffer_capacity_bytes - self._write_pos
+            if n <= end_space:
+                self._buffer[self._write_pos:self._write_pos + n] = data
+                self._write_pos += n
+                if self._write_pos == self._buffer_capacity_bytes:
+                    self._write_pos = 0
+                    self._has_wrapped = True
+            else:
+                self._buffer[self._write_pos:] = data[:end_space]
+                rest = n - end_space
+                self._buffer[0:rest] = data[end_space:]
+                self._write_pos = rest
+                self._has_wrapped = True
+
+    def _buffer_write_callback(self, in_data, frame_count, time_info, status):
+        if in_data:
+            self._write_to_buffer(in_data)
+            if self._recording:
+                with self._lock:
+                    self.recorded_frames.append(in_data)
+                    self.num_recorded_buffers = self.num_recorded_buffers+1
+        return (None, pyaudio.paContinue)
+
+    def _recorder_loop(self):
+        while self._listening:
+            loop_start_time = time.perf_counter()
+            # print(f"rms: {self.get_rms()}")
+            if not self._recording:
+                if self.get_rms() > self.RMS_THRESHOLD:
+                    self._recording = True
+                    pre_roll_data = self.get_last_recorded_bytes(2.0)
+                    with self._lock:
+                        self.recorded_frames = [pre_roll_data]
+            else:
+
+                if self.get_rms() < self.RMS_THRESHOLD:
+                    if self.first_timestamp_below_threshold is None:
+                        self.first_timestamp_below_threshold = time.time()
+                    elif time.time() - self.first_timestamp_below_threshold > 2.0:
+                        self._recording = False
+                        self.first_timestamp_below_threshold = None
+                        # TUTAJ WHISPER BIERZE RECORDED FRAMES
+                        with self._lock:
+                            audio_data = b''.join(self.recorded_frames)
+                            self.recorded_frames = []
+
+                        print("Transcribing recorded audio...")
+                        threading.Thread(
+                            target=self._transcribe_audio, 
+                            args=(audio_data,)
+                        ).start()
+                else:
+                    self.first_timestamp_below_threshold = None
+                        
+            loop_execution_time = time.perf_counter() - loop_start_time 
+            time.sleep(self.recording_loop_delay - loop_execution_time)
+
+
+    def _transcribe_audio(self, audio_data: bytes) -> str:
+        print(f"Buffer counter: {self.num_recorded_buffers}")
+        # ONLY FOR NOW - TO AVOID SHORT WHEEL NOISES
+        if self.num_recorded_buffers < 200: # Check for minimum audio length
+            print("Audio data too short to transcribe.")
+            return
+        self.num_recorded_buffers = 0
+        ram_buffer = io.BytesIO()
+        ram_buffer.name = "recorded.wav"
+        with wave.open(ram_buffer, "wb") as wf:
+            wf.setnchannels(self.CHANNELS)
+            wf.setsampwidth(self._sample_width)
+            wf.setframerate(self.RATE)
+            wf.writeframes(audio_data)
+        ram_buffer.seek(0)
+
+
+        transcription = self.openai_client.audio.transcriptions.create(
+            model="gpt-4o-transcribe", 
+            file=ram_buffer
+        )
+        if transcription.text:  # If transcription is not ""
+            print(f"transcription: {transcription.text}")
+            if self.wakeword in transcription.text.lower():
+                self.task_queue.put(transcription.text)
+
+
+    def start_listening(self):
+        print(f"Starting SoundReceiver on device index {self.DEVICE_INDEX}")
+        if self._listening:
+            return
+        try:
+            self._stream = self._p.open(format=self.FORMAT,
+                                       channels=self.CHANNELS,
+                                       rate=self.RATE,
+                                       input=True,
+                                       input_device_index=self.DEVICE_INDEX,
+                                       frames_per_buffer=self.frames_per_buffer,
+                                       stream_callback=self._buffer_write_callback)
+        except Exception as e:
+            raise RuntimeError(f"Failed to open input stream: {e}")
+        self._stream.start_stream()
+        self._listening = True
+        self.reciver_thread.start()
+
+    def stop(self):
+        if not self._listening:
+            return
+        try:
+            if self._stream is not None:
+                self._stream.stop_stream()
+                self._stream.close()
+                self._stream = None
+        finally:
+            try:
+                self._p.terminate()
+            except Exception:
+                pass
+            self._listening = False
+
+    def is_listening(self):
+        return self._listening
+
+    def get_buffer_bytes(self) -> bytes:
+        with self._lock:
+            if not self._has_wrapped:
+                return bytes(self._buffer[:self._write_pos])
+            return bytes(self._buffer[self._write_pos:] + self._buffer[:self._write_pos])
+
+    def get_last_recorded_bytes(self, seconds: float) -> bytes:
+        bytes_needed = int(min(seconds * self._bytes_per_second, self._buffer_capacity_bytes))
+        data = self.get_buffer_bytes()
+        if len(data) <= bytes_needed:
+            return data
+        return data[-bytes_needed:]
+
+    # RMS helpers
+    def get_rms(self) -> float:
+        """Return RMS level for raw PCM bytes (paInt16 width expected)."""
+        return float(audioop.rms(self.get_last_recorded_bytes(seconds=0.2), self._sample_width))
+    
+
+# Minimal CLI demo
+if __name__ == "__main__":
+    rec = SoundReceiver()
+    try:
+        # optionally set DEVICE_INDEX env var before running, or pass device_index to start_listening
+        rec.start_listening()  # non-blocking
+        input("Recording... press Enter to stop and save buffer to recent_from_buffer.wav\n")
+        print("Saving last 5 seconds to recent_from_buffer.wav")
+
+    finally:
+        rec.stop()

robocrew/core/tools.py

@@ -0,0 +1,8 @@
+from langchain_core.tools import tool
+
+
+@tool
+def finish_task():
+    """Claim that task is finished and go idle. You need to ensure the task is actually finished before calling this tool."""
+    return "Task finished"
+

robocrew/core/utils.py

@@ -0,0 +1,40 @@
+import cv2
+import math
+
+
+def horizontal_angle_grid(image, h_fov=120, center_angle=0):
+    """Draw horizontal angle markers on the bottom of the image."""
+    height, width = image.shape[:2]
+    yellow = (0, 255, 255)
+    orange = (0, 100, 255)
+    y_pos = 25
+    mark_len_angle = 10
+
+    # Draw baseline
+    cv2.line(image, (0, y_pos), (width, y_pos), yellow, 2)
+
+    # Generate markers every 5° within visible range
+    nr_of_marks = int((h_fov / 2) // mark_len_angle * 2 + 1)
+    pixels_per_mark = width / h_fov * mark_len_angle
+    start_pixel = (width - (nr_of_marks - 1) * pixels_per_mark) / 2
+    start_angle = (-h_fov / 2 + center_angle)
+    start_angle = mark_len_angle * math.trunc(start_angle / mark_len_angle)
+
+    for mark_number in range(nr_of_marks):
+        x = int(start_pixel + mark_number * pixels_per_mark)
+        angle = start_angle + mark_number * mark_len_angle
+        cv2.line(image, (x, y_pos - 10), (x, y_pos + 10), yellow, 2)
+        cv2.putText(image, f"{angle}", (x - 15, y_pos + 25),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.6, yellow, 2)
+        
+    # put right/left text
+    cv2.putText(image, "<=LEFT", (10, height - 10), cv2.FONT_HERSHEY_SIMPLEX, 1, orange, 2)
+    cv2.putText(image, "RIGHT=>", (width - 145, height - 10), cv2.FONT_HERSHEY_SIMPLEX, 1, yellow, 2)
+    return image
+
+if __name__ == "__main__":
+    # Test the function with a sample image
+    img = cv2.imread("img.png")
+    img_with_grid = horizontal_angle_grid(img, h_fov=118)
+    # write to file
+    cv2.imwrite("img_with_grid.jpg", img_with_grid)

robocrew/robots/XLeRobot/sdk.py

@@ -0,0 +1,410 @@
+import time
+from typing import Dict, List, Optional
+
+import serial  # type: ignore[import]
+import serial.tools.list_ports  # type: ignore[import]
+
+# Servo protocol constants ----------------------------------------------------
+
+BROADCAST_ID = 0xFE
+
+INST_WRITE = 3
+INST_SYNC_WRITE = 0x83
+
+# Comm results
+COMM_SUCCESS = 0
+COMM_PORT_BUSY = -1
+COMM_TX_FAIL = -2
+COMM_RX_FAIL = -3
+COMM_TX_ERROR = -4
+COMM_RX_WAITING = -5
+COMM_RX_TIMEOUT = -6
+COMM_RX_CORRUPT = -7
+COMM_NOT_AVAILABLE = -9
+
+TXPACKET_MAX_LEN = 250
+RXPACKET_MAX_LEN = 250
+
+# Packet positions
+PKT_HEADER0 = 0
+PKT_HEADER1 = 1
+PKT_ID = 2
+PKT_LENGTH = 3
+PKT_INSTRUCTION = 4
+PKT_ERROR = 4
+PKT_PARAMETER0 = 5
+
+# Control table addresses
+ADDR_SCS_GOAL_SPEED = 46
+ADDR_SCS_MODE = 33
+ADDR_SCS_LOCK = 55
+
+DEFAULT_BAUDRATE = 1_000_000
+LATENCY_TIMER = 16
+
+# Endianness flag like JS (STS/SMS=0, SCS=1). Most STS/SMS use 0.
+SCS_END = 0
+
+def SCS_LOBYTE(w: int) -> int:
+    return (w & 0xFF) if SCS_END == 0 else ((w >> 8) & 0xFF)
+
+
+def SCS_HIBYTE(w: int) -> int:
+    return ((w >> 8) & 0xFF) if SCS_END == 0 else (w & 0xFF)
+
+
+class PortHandler:
+    def __init__(self, port: Optional[str] = None, baudrate: int = DEFAULT_BAUDRATE):
+        self.port_name = port
+        self.baudrate = baudrate
+        self.ser: Optional[serial.Serial] = None
+        self.is_open = False
+        self.is_using = False
+        self.packet_start_time = 0.0
+        self.packet_timeout = 0.0
+        self.tx_time_per_byte = 0.0
+
+    def set_port(self, port: str):
+        self.port_name = port
+
+    def set_baudrate(self, baudrate: int):
+        self.baudrate = baudrate
+        self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0
+
+    def open_port(self) -> bool:
+        if not self.port_name:
+            return False
+        try:
+            self.ser = serial.Serial(self.port_name, self.baudrate, timeout=0, write_timeout=1)
+            self.is_open = True
+            self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0
+            return True
+        except Exception:
+            self.ser = None
+            self.is_open = False
+            return False
+
+    def close_port(self):
+        try:
+            if self.ser and self.ser.is_open:
+                self.ser.close()
+        finally:
+            self.is_open = False
+            self.ser = None
+
+    def clear_port(self):
+        if self.ser:
+            self.ser.reset_input_buffer()
+            self.ser.reset_output_buffer()
+
+    def write_port(self, data: List[int]) -> int:
+        if not (self.is_open and self.ser):
+            return 0
+        try:
+            out = bytes(byte & 0xFF for byte in data)
+            return self.ser.write(out)
+        except Exception:
+            return 0
+
+    def read_port(self, length: int) -> List[int]:
+        if not (self.is_open and self.ser):
+            return []
+        result: List[int] = []
+        start = time.time()
+        total_timeout = 0.5  # seconds
+        while len(result) < length:
+            if (time.time() - start) > total_timeout:
+                break
+            try:
+                chunk = self.ser.read(length - len(result))
+                if chunk:
+                    result.extend(chunk)
+                else:
+                    time.sleep(0.01)
+            except Exception:
+                break
+        return list(result)
+
+    def set_packet_timeout(self, packet_length: int):
+        self.packet_start_time = time.time()
+        self.packet_timeout = (self.tx_time_per_byte * packet_length + LATENCY_TIMER * 2.0 + 2.0) / 1000.0
+
+    def set_packet_timeout_millis(self, msec: float):
+        self.packet_start_time = time.time()
+        self.packet_timeout = msec / 1000.0
+
+    def is_packet_timeout(self) -> bool:
+        if (time.time() - self.packet_start_time) > self.packet_timeout:
+            self.packet_timeout = 0.0
+            return True
+        return False
+
+
+class PacketHandler:
+    def __init__(self, protocol_end: int = 0):
+        global SCS_END
+        SCS_END = protocol_end
+
+    def get_tx_rx_result(self, result: int) -> str:
+        mapping = {
+            COMM_SUCCESS: "Communication success",
+            COMM_PORT_BUSY: "Port in use",
+            COMM_TX_FAIL: "TX failed",
+            COMM_RX_FAIL: "RX failed",
+            COMM_TX_ERROR: "TX packet error",
+            COMM_RX_WAITING: "RX waiting",
+            COMM_RX_TIMEOUT: "RX timeout",
+            COMM_RX_CORRUPT: "RX corrupt",
+            COMM_NOT_AVAILABLE: "Not available",
+        }
+        return mapping.get(result, str(result))
+
+    def tx_packet(self, port: PortHandler, txpacket: List[int]) -> int:
+        checksum = 0
+        total_len = txpacket[PKT_LENGTH] + 4
+        if port.is_using:
+            return COMM_PORT_BUSY
+        port.is_using = True
+
+        if total_len > TXPACKET_MAX_LEN:
+            port.is_using = False
+            return COMM_TX_ERROR
+
+        txpacket[PKT_HEADER0] = 0xFF
+        txpacket[PKT_HEADER1] = 0xFF
+
+        for idx in range(2, total_len - 1):
+            checksum += txpacket[idx]
+        txpacket[total_len - 1] = (~checksum) & 0xFF
+
+        port.clear_port()
+        written = port.write_port(txpacket)
+        if written != total_len:
+            port.is_using = False
+            return COMM_TX_FAIL
+        return COMM_SUCCESS
+
+    def rx_packet(self, port: PortHandler):
+        rxpacket: List[int] = []
+        result = COMM_RX_FAIL
+        wait_length = 6
+
+        while True:
+            data = port.read_port(wait_length - len(rxpacket))
+            rxpacket.extend(data)
+
+            if len(rxpacket) >= wait_length:
+                header_index = -1
+                for i in range(0, len(rxpacket) - 1):
+                    if rxpacket[i] == 0xFF and rxpacket[i + 1] == 0xFF:
+                        header_index = i
+                        break
+                if header_index == 0:
+                    if rxpacket[PKT_ID] > 0xFD or rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN:
+                        rxpacket.pop(0)
+                        continue
+                    if wait_length != (rxpacket[PKT_LENGTH] + PKT_LENGTH + 1):
+                        wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1
+                        continue
+                    if len(rxpacket) < wait_length:
+                        if port.is_packet_timeout():
+                            result = COMM_RX_TIMEOUT if len(rxpacket) == 0 else COMM_RX_CORRUPT
+                            break
+                        continue
+                    checksum = 0
+                    for i in range(2, wait_length - 1):
+                        checksum += rxpacket[i]
+                    checksum = (~checksum) & 0xFF
+                    result = COMM_SUCCESS if rxpacket[wait_length - 1] == checksum else COMM_RX_CORRUPT
+                    break
+                elif header_index > 0:
+                    rxpacket = rxpacket[header_index:]
+                    continue
+            if port.is_packet_timeout():
+                result = COMM_RX_TIMEOUT if len(rxpacket) == 0 else COMM_RX_CORRUPT
+                break
+        return rxpacket, result
+
+    def tx_rx_packet(self, port: PortHandler, txpacket: List[int]):
+        rxpacket: Optional[List[int]] = None
+        error = 0
+        result = self.tx_packet(port, txpacket)
+        if result != COMM_SUCCESS:
+            port.is_using = False
+            return rxpacket, result, error
+
+        if txpacket[PKT_ID] == BROADCAST_ID:
+            port.is_using = False
+            return rxpacket, result, error
+
+        port.set_packet_timeout(10)
+
+        port.clear_port()
+        rxpacket, rx_res = self.rx_packet(port)
+
+        if rx_res != COMM_SUCCESS or not rxpacket:
+            port.is_using = False
+            return rxpacket, rx_res, error
+
+        if len(rxpacket) < 6 or rxpacket[PKT_ID] != txpacket[PKT_ID]:
+            port.is_using = False
+            return rxpacket, COMM_RX_CORRUPT, error
+
+        error = rxpacket[PKT_ERROR]
+        port.is_using = False
+        return rxpacket, rx_res, error
+
+    # Write helpers
+    def write_tx_rx(self, port: PortHandler, scs_id: int, address: int, data: List[int]):
+        if scs_id >= BROADCAST_ID:
+            return COMM_NOT_AVAILABLE, 0
+        txpacket = [0] * (len(data) + 7)
+        txpacket[PKT_ID] = scs_id
+        txpacket[PKT_LENGTH] = len(data) + 3
+        txpacket[PKT_INSTRUCTION] = INST_WRITE
+        txpacket[PKT_PARAMETER0] = address
+        for i, value in enumerate(data):
+            txpacket[PKT_PARAMETER0 + 1 + i] = value & 0xFF
+        rxpacket, result, error = self.tx_rx_packet(port, txpacket)
+        return result, error
+
+    def write1(self, port: PortHandler, scs_id: int, address: int, data: int):
+        return self.write_tx_rx(port, scs_id, address, [data & 0xFF])
+
+    def write2(self, port: PortHandler, scs_id: int, address: int, data: int):
+        arr = [SCS_LOBYTE(data), SCS_HIBYTE(data)]
+        return self.write_tx_rx(port, scs_id, address, arr)
+
+    # Sync ops (TX only)
+    def sync_write_tx_only(self, port: PortHandler, start_address: int, data_len: int, param: List[int]) -> int:
+        txpacket = [0] * (len(param) + 8)
+        txpacket[PKT_ID] = BROADCAST_ID
+        txpacket[PKT_LENGTH] = len(param) + 4
+        txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE
+        txpacket[PKT_PARAMETER0] = start_address
+        txpacket[PKT_PARAMETER0 + 1] = data_len
+        for i, b in enumerate(param):
+            txpacket[PKT_PARAMETER0 + 2 + i] = b & 0xFF
+        # build checksum and send
+        res = self.tx_packet(port, txpacket)
+        port.is_using = False
+        return res
+
+
+class GroupSyncWrite:
+    def __init__(self, port: PortHandler, ph: PacketHandler, start_address: int, data_length: int):
+        self.port = port
+        self.ph = ph
+        self.start_address = start_address
+        self.data_length = data_length
+        self.ids: List[int] = []
+        self.data: Dict[int, List[int]] = {}
+
+    def add_param(self, scs_id: int, data: List[int]) -> bool:
+        if scs_id in self.ids:
+            return False
+        if len(data) != self.data_length:
+            return False
+        self.ids.append(scs_id)
+        self.data[scs_id] = list(data)
+        return True
+
+    def clear_param(self):
+        self.ids.clear()
+        self.data.clear()
+
+    def make_param(self) -> List[int]:
+        param: List[int] = []
+        for sid in self.ids:
+            param.append(sid)
+            param.extend(self.data[sid])
+        return param
+
+    def tx_packet(self) -> int:
+        if not self.ids:
+            return COMM_NOT_AVAILABLE
+        param = self.make_param()
+        return self.ph.sync_write_tx_only(self.port, self.start_address, self.data_length, param)
+
+
+class ScsServoSDK:
+    """Thin convenience wrapper for the handful of operations we need."""
+
+    def __init__(self):
+        self.port = PortHandler()
+        self.ph = PacketHandler(0)
+
+    @staticmethod
+    def list_ports() -> List[str]:
+        return [p.device for p in serial.tools.list_ports.comports()]
+
+    def connect(self, port: str, baudrate: int = DEFAULT_BAUDRATE, protocol_end: int = 0) -> bool:
+        self.port.set_port(port)
+        self.port.set_baudrate(baudrate)
+        self.ph = PacketHandler(protocol_end)
+        return self.port.open_port()
+
+    def disconnect(self):
+        self.port.close_port()
+
+    # ------------------------------------------------------------------
+    # Basic write helpers
+    # ------------------------------------------------------------------
+
+    def _write_byte(self, servo_id: int, address: int, value: int):
+        res, err = self.ph.write1(self.port, servo_id, address, value)
+        if res != COMM_SUCCESS:
+            raise RuntimeError(f"write_byte failed: {self.ph.get_tx_rx_result(res)} err={err}")
+
+    def _write_word(self, servo_id: int, address: int, value: int):
+        res, err = self.ph.write2(self.port, servo_id, address, value)
+        if res != COMM_SUCCESS:
+            raise RuntimeError(f"write_word failed: {self.ph.get_tx_rx_result(res)} err={err}")
+
+    # ------------------------------------------------------------------
+    # Wheel helpers
+    # ------------------------------------------------------------------
+
+    def set_wheel_mode(self, servo_id: int) -> str:
+        self._unlock(servo_id)
+        try:
+            self._write_byte(servo_id, ADDR_SCS_MODE, 1)
+        finally:
+            self._lock(servo_id)
+        return "success"
+
+    def write_wheel_speed(self, servo_id: int, speed: int) -> str:
+        speed = max(-10000, min(10000, int(speed)))
+        value = abs(speed) & 0x7FFF
+        if speed < 0:
+            value |= 0x8000
+        self._write_word(servo_id, ADDR_SCS_GOAL_SPEED, value)
+        return "success"
+
+    def sync_write_wheel_speeds(self, servo_speeds: Dict[int, int]) -> str:
+        if not servo_speeds:
+            return "success"
+        group = GroupSyncWrite(self.port, self.ph, ADDR_SCS_GOAL_SPEED, 2)
+        added = False
+        for sid, speed in servo_speeds.items():
+            speed = max(-10000, min(10000, int(speed)))
+            value = abs(speed) & 0x7FFF
+            if speed < 0:
+                value |= 0x8000
+            added = group.add_param(int(sid), [SCS_LOBYTE(value), SCS_HIBYTE(value)]) or added
+        if not added:
+            return "success"
+        result = group.tx_packet()
+        if result != COMM_SUCCESS:
+            raise RuntimeError(f"sync_write_wheel_speeds failed: {self.ph.get_tx_rx_result(result)}")
+        return "success"
+
+    # ------------------------------------------------------------------
+    # Internal helpers
+    # ------------------------------------------------------------------
+
+    def _unlock(self, servo_id: int):
+        self._write_byte(servo_id, ADDR_SCS_LOCK, 0)
+
+    def _lock(self, servo_id: int):
+        self._write_byte(servo_id, ADDR_SCS_LOCK, 1)

robocrew/robots/XLeRobot/tools.py

@@ -0,0 +1,43 @@
+import sys
+from pathlib import Path
+from langchain_core.tools import tool  # type: ignore[import]
+import time
+
+
+def create_move_forward(wheel_controller):
+    @tool
+    def move_forward(distance_meters: float) -> str:
+        """Drives the robot forward (or backward) for a specific distance."""
+
+        distance = float(distance_meters)
+        if distance >= 0:
+            wheel_controller.go_forward(distance)
+        else:
+            wheel_controller.go_backward(-distance)
+        return f"Moved {'forward' if distance >= 0 else 'backward'} {abs(distance):.2f} meters."
+
+    return move_forward
+
+
+def create_turn_right(wheel_controller):
+    @tool
+    def turn_right(angle_degrees: float) -> str:
+        """Turns the robot right by angle in degrees."""
+        angle = float(angle_degrees)
+        wheel_controller.turn_right(angle)
+        time.sleep(0.4)  # wait a bit after turn for stabilization
+        return f"Turned right by {angle} degrees."
+
+    return turn_right
+
+
+def create_turn_left(wheel_controller):
+    @tool
+    def turn_left(angle_degrees: float) -> str:
+        """Turns the robot left by angle in degrees."""
+        angle = float(angle_degrees)
+        wheel_controller.turn_left(angle)
+        time.sleep(0.4)  # wait a bit after turn for stabilization
+        return f"Turned left by {angle} degrees."
+
+    return turn_left

robocrew/robots/XLeRobot/wheel_controls.py

@@ -0,0 +1,284 @@
+
+"""High-level wheel helpers for the XLe robot.
+
+The original setup pulled its configuration from ``dual_mapper_config.json``.
+To simplify deployment the relevant wheel parameters now live directly in this
+module so there is no external configuration file dependency.
+"""
+
+from __future__ import annotations
+
+import time
+from dataclasses import dataclass, field
+from typing import Dict, List, Mapping, Optional, Sequence, Tuple, Union
+
+from robocrew.robots.XLeRobot.sdk import DEFAULT_BAUDRATE, ScsServoSDK
+
+
+# ---------------------------------------------------------------------------
+# Default wheel configuration (was ``portB.wheel`` in dual_mapper_config.json)
+# ---------------------------------------------------------------------------
+
+
+@dataclass(frozen=True)
+class WheelCalibration:
+    """Direction multipliers applied to the configured base speed."""
+
+    up: int = 0
+    down: int = 0
+    left: int = 0
+    right: int = 0
+
+    @classmethod
+    def from_mapping(cls, data: Mapping[str, int]) -> "WheelCalibration":
+        return cls(
+            up=int(data.get("Up", 0)),
+            down=int(data.get("Down", 0)),
+            left=int(data.get("Left", 0)),
+            right=int(data.get("Right", 0)),
+        )
+
+    def value_for(self, action: str) -> int:
+        lookup = {
+            "up": self.up,
+            "down": self.down,
+            "left": self.left,
+            "right": self.right,
+        }
+        return lookup.get(action.lower(), 0)
+
+
+@dataclass(frozen=True)
+class WheelSpec:
+    """Single wheel definition with its role and calibration."""
+
+    id: int
+    role: str = "both"
+    calibration: WheelCalibration = field(default_factory=WheelCalibration)
+
+    @classmethod
+    def from_mapping(cls, data: Mapping[str, object]) -> Optional["WheelSpec"]:
+        wid = int(data.get("id", 0))
+        if wid <= 0:
+            return None
+        role = str(data.get("role", "both")).lower()
+        raw_cal = data.get("calibration", {})
+        if isinstance(raw_cal, Mapping):
+            calibration = WheelCalibration.from_mapping(raw_cal)
+        else:
+            calibration = WheelCalibration()
+        return cls(id=wid, role=role, calibration=calibration)
+
+    def allows(self, action: str) -> bool:
+        action = action.lower()
+        if self.role == "both":
+            return action in ("up", "down", "left", "right")
+        if self.role == "drive":
+            return action in ("up", "down")
+        if self.role == "steer":
+            return action in ("left", "right")
+        return False
+
+    def speed_for(self, action: str, base_speed: int) -> int:
+        if not self.allows(action):
+            return 0
+        return self.calibration.value_for(action) * base_speed
+
+
+@dataclass(frozen=True)
+class WheelConfig:
+    """Full wheel configuration shared by all actions."""
+
+    speed: int
+    meters_per_second: float
+    degrees_per_second: float
+    wheels: Tuple[WheelSpec, ...] = field(default_factory=tuple)
+
+    @classmethod
+    def from_mapping(cls, data: Mapping[str, object]) -> "WheelConfig":
+        speed = int(data.get("speed", DEFAULT_WHEEL_CONFIG.speed))
+        meters_per_second = float(data.get("meters_per_second", DEFAULT_WHEEL_CONFIG.meters_per_second))
+        degrees_per_second = float(data.get("degrees_per_second", DEFAULT_WHEEL_CONFIG.degrees_per_second))
+        wheels: List[WheelSpec] = []
+        for entry in data.get("wheels", []):
+            if isinstance(entry, Mapping):
+                spec = WheelSpec.from_mapping(entry)
+                if spec:
+                    wheels.append(spec)
+        return cls(
+            speed=speed,
+            meters_per_second=meters_per_second,
+            degrees_per_second=degrees_per_second,
+            wheels=tuple(wheels),
+        )
+
+
+DEFAULT_WHEEL_CONFIG = WheelConfig(
+    speed=10000,
+    meters_per_second=0.25,
+    degrees_per_second=100.0,
+    wheels=(
+        WheelSpec(
+            id=7,
+            role="both",
+            calibration=WheelCalibration(up=1, down=-1, left=-1, right=1),
+        ),
+        WheelSpec(
+            id=8,
+            role="steer",
+            calibration=WheelCalibration(up=0, down=0, left=-1, right=1),
+        ),
+        WheelSpec(
+            id=9,
+            role="both",
+            calibration=WheelCalibration(up=-1, down=1, left=-1, right=1),
+        ),
+    ),
+)
+
+
+def build_wheel_config(
+    *,
+    speed: Optional[int] = None,
+    meters_per_second: Optional[float] = None,
+    degrees_per_second: Optional[float] = None,
+    wheels: Optional[Sequence[Mapping[str, object]]] = None,
+) -> WheelConfig:
+    """Helper to create a ``WheelConfig`` from simple primitives."""
+
+    base = DEFAULT_WHEEL_CONFIG
+    resolved_speed = base.speed if speed is None else int(speed)
+    resolved_mps = base.meters_per_second if meters_per_second is None else float(meters_per_second)
+    resolved_dps = base.degrees_per_second if degrees_per_second is None else float(degrees_per_second)
+
+    if wheels is None:
+        resolved_wheels = base.wheels
+    else:
+        collected: List[WheelSpec] = []
+        for entry in wheels:
+            spec = WheelSpec.from_mapping(entry)
+            if spec:
+                collected.append(spec)
+        resolved_wheels = tuple(collected)
+
+    return WheelConfig(
+        speed=resolved_speed,
+        meters_per_second=resolved_mps,
+        degrees_per_second=resolved_dps,
+        wheels=resolved_wheels,
+    )
+
+
+# ---------------------------------------------------------------------------
+# Controller
+# ---------------------------------------------------------------------------
+
+
+class XLeRobotWheels:
+    def __init__(
+        self,
+        sdk: ScsServoSDK,
+        wheel_cfg: Optional[Union[WheelConfig, Mapping[str, object]]] = None,
+    ):
+        self.sdk = sdk
+        if wheel_cfg is None:
+            self.config = DEFAULT_WHEEL_CONFIG
+        elif isinstance(wheel_cfg, WheelConfig):
+            self.config = wheel_cfg
+        elif isinstance(wheel_cfg, Mapping):
+            self.config = WheelConfig.from_mapping(wheel_cfg)
+        else:
+            raise TypeError("wheel_cfg must be a WheelConfig or mapping")
+        if not self.config.wheels:
+            raise ValueError("Wheel configuration must define at least one wheel.")
+        self.linear_mps = max(1e-6, float(self.config.meters_per_second))
+        self.angular_dps = max(1e-6, float(self.config.degrees_per_second))
+
+    def _apply_action(self, action: str) -> Dict[int, int]:
+        payload = {wheel.id: wheel.speed_for(action, self.config.speed) for wheel in self.config.wheels}
+        if payload:
+            self.sdk.sync_write_wheel_speeds(payload)
+        return payload
+
+    def _stop_all(self) -> Dict[int, int]:
+        payload = {wheel.id: 0 for wheel in self.config.wheels}
+        if payload:
+            self.sdk.sync_write_wheel_speeds(payload)
+        return payload
+
+    def _run_for(self, action: str, duration_s: float) -> Dict[int, int]:
+        duration = max(0.0, float(duration_s))
+        self._apply_action(action)
+        time.sleep(duration)
+        self._stop_all()
+        
+
+    def _distance_to_duration(self, distance_m: float) -> float:
+        return abs(float(distance_m)) / self.linear_mps
+
+    def _angle_to_duration(self, degrees: float) -> float:
+        return abs(float(degrees)) / self.angular_dps
+
+    def go_forward(self, meters: float) -> Dict[int, int]:
+        """Drive forward for the requested distance in meters."""
+
+        distance = float(meters)
+        if distance < 0:
+            return self.go_backward(-distance)
+        if distance == 0:
+            return {}
+        return self._run_for("Up", self._distance_to_duration(distance))
+
+    def go_backward(self, meters: float) -> Dict[int, int]:
+        """Drive backward for the requested distance in meters."""
+
+        distance = float(meters)
+        if distance < 0:
+            return self.go_forward(-distance)
+        if distance == 0:
+            return {}
+        return self._run_for("Down", self._distance_to_duration(distance))
+
+    def turn_left(self, degrees: float) -> Dict[int, int]:
+        """Turn left by the requested angle in degrees."""
+
+        angle = float(degrees)
+        if angle < 0:
+            return self.turn_right(-angle)
+        if angle == 0:
+            return {}
+        return self._run_for("Left", self._angle_to_duration(angle))
+
+    def turn_right(self, degrees: float) -> Dict[int, int]:
+        """Turn right by the requested angle in degrees."""
+
+        angle = float(degrees)
+        if angle < 0:
+            return self.turn_left(-angle)
+        if angle == 0:
+            return {}
+        return self._run_for("Right", self._angle_to_duration(angle))
+
+    def apply_wheel_modes(self) -> List[int]:
+        """Switch all configured motors to wheel mode."""
+
+        applied: List[int] = []
+        for wheel in self.config.wheels:
+            self.sdk.set_wheel_mode(wheel.id)
+            applied.append(wheel.id)
+        return applied
+
+    @staticmethod
+    def connect_serial(
+        port: str,
+        baudrate: int = DEFAULT_BAUDRATE,
+        protocol_end: int = 0,
+    ) -> ScsServoSDK:
+        """Create and connect an ``ScsServoSDK`` instance."""
+
+        sdk = ScsServoSDK()
+        if not sdk.connect(port, baudrate, protocol_end):
+            raise RuntimeError(f"Failed to open serial port {port} @ {baudrate}")
+        return sdk
+
+

robocrew-0.0.1.dist-info/METADATA

@@ -0,0 +1,19 @@
+Metadata-Version: 2.4
+Name: robocrew
+Version: 0.0.1
+Summary: 🦾Set up your embodied LLM agent same easiness as you set up normal agents in CrewAI or Autogen
+Project-URL: Homepage, https://github.com/Grigorij-Dudnik/RoboCrew
+Author: Konrad Suchodolski
+Author-email: Grigorij Dudnik <dudnikgrv@gmail.com>
+License-Expression: MIT
+Classifier: Operating System :: OS Independent
+Classifier: Programming Language :: Python :: 3
+Requires-Python: >=3.10
+Requires-Dist: langchain-core>=0.3.79
+Requires-Dist: langchain-google-genai
+Requires-Dist: langchain>=0.3.27
+Requires-Dist: openai>=2.3.0
+Requires-Dist: opencv-python
+Requires-Dist: pyaudio
+Requires-Dist: pyserial>=3.5
+Requires-Dist: python-dotenv>=1.1.1

robocrew-0.0.1.dist-info/RECORD

@@ -0,0 +1,12 @@
+robocrew/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+robocrew/core/LLMAgent.py,sha256=oHhsPJlH4yhdHC4qe9Jy0SNxxaXgxeB1ezPI3ss0QdE,5633
+robocrew/core/sound_receiver.py,sha256=Mu_6McbyyqYiclJwcMCJGuJHZX3Pe5YLw9zBA2m9ULY,8102
+robocrew/core/tools.py,sha256=nXkXln1d7BMhuU1125T_Iz1zqXnWhLmqp5P4PvTEzGg,227
+robocrew/core/utils.py,sha256=8s5vdP5IfUVtL9PZPCSWjDDSdBTsk1Y6rtMlOqYvNQ8,1598
+robocrew/robots/XLeRobot/dual_mapper_config.json,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+robocrew/robots/XLeRobot/sdk.py,sha256=SiNLLQA-iGQY69z18OhpMNMdWRmLv4zEpZpbrGIXsa8,14217
+robocrew/robots/XLeRobot/tools.py,sha256=waV4uqPodwneICYd2WB0DrzFovj8YTPU2VUG8wGGino,1400
+robocrew/robots/XLeRobot/wheel_controls.py,sha256=2MHm_zCj5nUpWXE5IiqvHN34d_G7pOvFrEsitPcpUBg,9690
+robocrew-0.0.1.dist-info/METADATA,sha256=U1Unk7Yat6hVUs3UHZ8tMYsn6MwcerkEDiDKfik_TVA,703
+robocrew-0.0.1.dist-info/WHEEL,sha256=qtCwoSJWgHk21S1Kb4ihdzI2rlJ1ZKaIurTj_ngOhyQ,87
+robocrew-0.0.1.dist-info/RECORD,,

robocrew-0.0.1.dist-info/WHEEL

@@ -0,0 +1,4 @@
+Wheel-Version: 1.0
+Generator: hatchling 1.27.0
+Root-Is-Purelib: true
+Tag: py3-none-any