Added actions endpoint multiple clients can use to simultaneously control actions

toid_cli/toid_cli/main.py

@@ -1,13 +1,42 @@
-import subprocess
-import uuid
-import os
-import signal
 from fastapi import FastAPI
 from pydantic import BaseModel
-import uvicorn
+from contextlib import asynccontextmanager
 from fastapi.middleware.cors import CORSMiddleware
+import asyncio
+import rclpy
+import uvicorn
+import threading
+import logging
 
-app = FastAPI()
+import toid_cli.routers.startup as startup
+import toid_cli.routers.action as action
+from toid_cli.services.services import actionClient, ServerRunner, log
+from toid_cli.services.runners import main_runner
+
+logging.getLogger().setLevel(logging.INFO)
+
+@asynccontextmanager
+async def lifespan(app: FastAPI):
+    rclpy.init()
+    ServerRunner.setRunner(ServerRunner(app))
+    thread = threading.Thread(target=rclpy.spin, args=(ServerRunner.getRunner(),), daemon=True)
+    thread.start()
+    log.info("Started up rclpy")
+    if not hasattr(app.state, 'loop'):
+        app.state.loop = asyncio.get_running_loop()
+
+    yield
+
+    await main_runner.stop_robot()
+    log.info("Stopped robot")
+    ServerRunner.getRunner().destroy_node()
+    rclpy.shutdown()
+    thread.join()
+    log.info("Closed rclpy")
+
+
+
+app = FastAPI(lifespan=lifespan)
 app.add_middleware(
     CORSMiddleware,
     allow_origins=[
@@ -17,84 +46,17 @@ app.add_middleware(
     allow_methods=["*"],
     allow_headers=["*"],
 )
+
+
+
+app.include_router(startup.router, prefix="/startup")
+app.include_router(action.router, prefix="/action")
 # store running launch processes
 launch_processes = {}
 
 
-class LaunchRequest(BaseModel):
-    package: str
-    launch_file: str
-    args: dict = {}
 
 
-@app.post("/launch/start")
-def start_launch(req: LaunchRequest):
-    launch_id = str(uuid.uuid4())
-
-    cmd = ["ros2", "launch", req.package, req.launch_file]
-
-    for k, v in req.args.items():
-        cmd.append(f"{k}:={v}")
-
-    log_dir = "log"
-
-    os.makedirs(log_dir, exist_ok=True)
-
-    stdout_file = open(f"{log_dir}/{launch_id}_stdout.log", "w")
-    stderr_file = open(f"{log_dir}/{launch_id}_stderr.log", "w")
-
-    proc = subprocess.Popen(
-        cmd,
-        stdout=stdout_file,
-        stderr=stderr_file,
-        preexec_fn=os.setsid
-    )
-
-    launch_processes[launch_id] = proc
-
-    return {
-        "launch_id": launch_id,
-        "pid": proc.pid,
-        "command": " ".join(cmd)
-    }
-
-
-@app.get("/launch/list")
-def list_launches():
-    running = {}
-
-    for lid, proc in launch_processes.items():
-        running[lid] = {
-            "pid": proc.pid,
-            "running": proc.poll() is None
-        }
-
-    return running
-
-
-@app.post("/launch/stop/{launch_id}")
-def stop_launch(launch_id: str):
-
-    if launch_id not in launch_processes:
-        return {"error": "launch id not found"}
-
-    proc = launch_processes[launch_id]
-
-    if proc.poll() is None:
-        os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
-
-    return {"stopped": launch_id}
-
-
-@app.post("/launch/stop_all")
-def stop_all():
-
-    for proc in launch_processes.values():
-        if proc.poll() is None:
-            os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
-
-    return {"status": "all stopped"}
-
 def main():
     uvicorn.run(
         "toid_cli.main:app",

toid_cli/toid_cli/routers/action.py

@@ -0,0 +1,88 @@
+from fastapi import APIRouter, WebSocket, WebSocketDisconnect, Request, status
+from fastapi.responses import JSONResponse
+from uvicorn.logging import ColourizedFormatter
+
+from rosbridge_library.internal.message_conversion import populate_instance, NonexistentFieldException, FieldTypeMismatchException
+from toid_msgs.action import SimpleRotate, SimpleTranslateX, SimpleMoveCoords
+
+import logging
+import json
+
+from toid_cli.services.services import ServerRunner
+
+router = APIRouter()
+log = logging.getLogger("ActionRoute")
+log.addHandler(logging.StreamHandler())
+log.handlers[0].setFormatter(ColourizedFormatter('%(levelprefix)s %(message)s'))
+log.setLevel(logging.INFO)
+
+@router.post("/start_tree")
+async def start_tree(req: Request):
+    tree = req.query_params.get("tree")
+    ServerRunner.getRunner().execute_tree(tree)
+    return JSONResponse("Works")
+
+@router.post("/rotate")
+async def rotate(req: Request):
+    try:
+        goal = await req.json()
+    except json.decoder.JSONDecodeError:
+        return JSONResponse("Bad Request", status_code=status.HTTP_400_BAD_REQUEST)
+    log.info(goal)
+    try:
+        goal = populate_instance(goal, SimpleRotate.Goal())
+        if not ServerRunner.getRunner().rotate_action(goal):
+            return JSONResponse("Bad Request", status_code=status.HTTP_503_SERVICE_UNAVAILABLE)
+    except (NonexistentFieldException, FieldTypeMismatchException) as e:
+        return JSONResponse("Bad Request", status_code=status.HTTP_400_BAD_REQUEST)
+    return JSONResponse("OK")
+
+@router.post("/translate")
+async def rotate(req: Request):
+    try:
+        goal = await req.json()
+    except json.decoder.JSONDecodeError:
+        return JSONResponse("Bad Request", status_code=status.HTTP_400_BAD_REQUEST)
+    log.info(goal)
+    try:
+        goal = populate_instance(goal, SimpleMoveCoords.Goal())
+        if not ServerRunner.getRunner().translate_coords(goal):
+            return JSONResponse("Server busy", status_code=status.HTTP_503_SERVICE_UNAVAILABLE)
+    except (NonexistentFieldException, FieldTypeMismatchException) as e:
+        return JSONResponse("Bad Request", status_code=status.HTTP_400_BAD_REQUEST)
+    return JSONResponse("OK")
+
+@router.post("/translate_x")
+async def rotate(req: Request):
+    try:
+        goal = await req.json()
+    except json.decoder.JSONDecodeError:
+        return JSONResponse("Bad Request", status_code=status.HTTP_400_BAD_REQUEST)
+    log.info(goal)
+    try:
+        goal = populate_instance(goal, SimpleTranslateX.Goal())
+        if not ServerRunner.getRunner().translate_x(goal):
+            return JSONResponse("Server busy", status_code=status.HTTP_503_SERVICE_UNAVAILABLE)
+    except (NonexistentFieldException, FieldTypeMismatchException) as e:
+        return JSONResponse("Bad Request", status_code=status.HTTP_400_BAD_REQUEST)
+    return JSONResponse("OK")
+
+@router.post("/start_tf_pub")
+async def startTFpub(req: Request):
+    if not ServerRunner.getRunner().start_tf_pub():
+        return JSONResponse("Server busy", status_code=status.HTTP_503_SERVICE_UNAVAILABLE)
+    return JSONResponse("OK")
+
+
+@router.websocket("/ws")
+async def websocketHandler(ws: WebSocket):
+    await ws.accept()
+    try:
+        ServerRunner.getRunner().add(ws)
+        while True:
+            data = await ws.receive_text()
+            log.info(data)
+            await ws.send_text("hello")
+    except WebSocketDisconnect: 
+        ServerRunner.getRunner().remove(ws)
+        log.warning("User disconnected")

toid_cli/toid_cli/routers/startup.py

@@ -0,0 +1,27 @@
+from fastapi import APIRouter, Request, Response
+from toid_cli.services.runners import main_runner
+
+router = APIRouter()
+
+@router.post("/run/robot")
+async def run_main(req: Request):
+    status = False
+    if(req.query_params.get("use_mock", False).lower() == "true"):
+        status = await main_runner.run_robot(use_mock=True)
+    else:
+        status = await main_runner.run_robot(use_mock=False)
+    if status:
+        return Response(status_code=200) 
+    return Response(status_code=400)
+
+@router.post("/stop/robot")
+async def run_main(req: Request):
+    status = await main_runner.stop_robot()
+    if status:
+        return Response(status_code=200) 
+    return Response(status_code=400)
+
+@router.get("/status/robot")
+async def run_main(req: Request):
+    status = "OK" if main_runner.status() else "NOT RUNNING"
+    return Response(content=status, status_code=200) 

toid_cli/toid_cli/services/runners.py

@@ -0,0 +1,47 @@
+import asyncio
+import asyncio.subprocess as subprocess
+import signal
+import os
+
+
+class Runner():
+    lock = asyncio.Lock()
+    running = False
+    running_type = ''
+    proc = None
+
+    async def run_robot(self, use_mock=False) -> bool:
+        async with self.lock:
+            if self.running:
+                return False
+            if use_mock == False:
+                cmd = ["ros2", "launch", "toid_navigation", "main.py", f"use_mock:={use_mock}"]
+            else:
+                cmd = ["ros2", "launch", "toid_navigation", "main.py", f"use_mock:={use_mock}"]
+            self.proc = await subprocess.create_subprocess_exec(
+                *cmd,
+                stdin=subprocess.PIPE,
+                stdout=subprocess.PIPE,
+                preexec_fn=os.setsid
+            )
+            self.running = True
+            return True
+
+    async def stop_robot(self):
+        async with self.lock:
+            if not self.running:
+                return False
+
+            os.killpg(os.getpgid(self.proc.pid), signal.SIGINT)
+            try:
+                await asyncio.wait_for(self.proc.wait(), timeout=8)
+            except asyncio.TimeoutError:
+                os.killpg(os.getpgid(self.proc.pid), signal.SIGKILL)
+            self.running = False
+            await self.proc.wait()
+            return True
+
+    def status(self):
+            return self.running
+
+main_runner = Runner()

toid_cli/toid_cli/services/services.py

@@ -0,0 +1,201 @@
+from rclpy import Future
+from rclpy.node import Node
+from rclpy.action import ActionClient
+from rclpy.action.client import ClientGoalHandle
+from rosbridge_library.internal.message_conversion import extract_values, populate_instance
+from btcpp_ros2_interfaces.action import ExecuteTree
+from toid_msgs.action import SimpleMoveCoords, SimpleRotate, SimpleTranslateX
+from action_msgs.srv import CancelGoal
+from tf2_web_republisher_interfaces.action import TFSubscription
+from uvicorn.logging import ColourizedFormatter
+from fastapi import WebSocket, FastAPI, WebSocketDisconnect
+import asyncio
+import logging
+import threading
+
+log = logging.getLogger("ActionClientHandler")
+log.addHandler(logging.StreamHandler())
+log.handlers[0].setFormatter(ColourizedFormatter('%(levelprefix)s %(message)s'))
+log.setLevel(logging.INFO)
+        
+class ActionClientHandler:
+    action_client: ActionClient
+    action_name: str
+    running: bool
+    connections: set[WebSocket]
+    app: FastAPI = None
+    lock: threading.Lock
+
+    def __init__(self, app: FastAPI, action_client: ActionClient):
+        self.action_client = action_client
+        self.action_name = action_client._action_name
+        self.app = app
+        self.lock = threading.Lock()
+        self.running = False
+        self.connections = set()
+
+    def send_goal(self, goal):
+        with self.lock:
+            if not self.action_client.wait_for_server(1):
+                log.info("Server doesn't exist yet")
+                self.running = False
+                return False
+            if self.running:
+                if not self.cancel_goals():
+                    return False
+            self.running = True
+
+        log.info("Sending goal")
+        future = self.action_client.send_goal_async(
+            goal=goal, 
+            feedback_callback=self.feedback)
+        future.add_done_callback(self.response_callback)
+        return True
+
+    def feedback(self, feedback):
+        #log.info("Feedback recieved")
+        self.sendMessage(
+            {
+                'type': 'goal_feedback', 
+                'name': self.action_name,
+                'message': extract_values(feedback.feedback),
+            })
+        
+    def response_callback(self, future: Future):
+        log.info("Goal response")
+        goal_handle: ClientGoalHandle = future.result()
+        if not goal_handle.accepted:
+            log.info("Goal not accepeted")
+            with self.lock:
+                self.running = False
+            self.sendMessage(
+                {
+                    'type': 'goal_accepted', 
+                    'name': self.action_name,
+                    'accepted': False,
+                })
+            return
+
+        log.info("Goal accepeted")
+
+        self.sendMessage(
+            {
+                'type': 'goal_accepted', 
+                'name': self.action_name,
+                'accepted': True,
+            })
+        f = goal_handle.get_result_async().add_done_callback(self.done)
+
+    def done(self, future: Future):
+        log.info("Goal done maybe")
+        result: ExecuteTree.Result = future.result().result
+
+        self.sendMessage(
+            {
+                'type': 'goal_done', 
+                'name': self.action_name,
+                'message': extract_values(result),
+            })
+
+        with self.lock:
+            self.running = False
+    
+    def sendMessage(self, msg: dict):
+        with self.lock:
+            connections = list(self.connections)
+        for conn in connections:
+            try:
+                asyncio.run_coroutine_threadsafe(
+                    conn.send_json(msg),
+                    self.app.state.loop
+                )
+            except WebSocketDisconnect:
+                pass
+    
+    def cancel_goals(self):
+        node: Node = self.action_client._node
+        cli = node.create_client(CancelGoal, self.action_name + "/_action/cancel_goal")
+        return cli.call(CancelGoal.Request(), 1.0)
+        
+
+    def status(self,):
+        with self.lock:
+            return self.running
+    
+    def addConnection(self, ws: WebSocket):
+        with self.lock:
+            self.connections.add(ws)
+
+    def removeConnection(self, ws: WebSocket):
+        with self.lock:
+            self.connections.remove(ws)
+    
+
+
+
+actionClient = None
+
+class ServerRunner(Node):
+    bt_action_client: ActionClientHandler
+    rotate: ActionClientHandler
+    move_coords: ActionClientHandler
+    move_x: ActionClientHandler
+    tf_web: ActionClientHandler
+    app: FastAPI = None
+
+    def __init__(self, app: FastAPI):
+        super().__init__("RestServerNode")
+        self.bt_action_client = ActionClientHandler(app, ActionClient(self, ExecuteTree, "/bt_run"))
+        self.rotate = ActionClientHandler(app, ActionClient(self, SimpleRotate, "/rotate"))
+        self.move_coords = ActionClientHandler(app, ActionClient(self, SimpleMoveCoords, "/moveCoords"))
+        self.move_x = ActionClientHandler(app, ActionClient(self, SimpleTranslateX, "/move_x"))
+        self.tf_web = ActionClientHandler(app, ActionClient(self, TFSubscription, "/tf2_web_republisher"))
+        self.app = None
+        
+    def start_tf_pub(self):
+        goal = TFSubscription.Goal()
+        goal.target_frame = "map"
+        goal.source_frames = ["base_footprint"]
+        goal.angular_thres = 0.0
+        goal.trans_thres = 0.0
+        goal.rate = 10.0
+        return self.tf_web.send_goal(goal)
+
+    def execute_tree(self, tree_name,):
+        goal = ExecuteTree.Goal(target_tree=tree_name)
+        return self.bt_action_client.send_goal(goal)
+
+    def rotate_action(self, goal):
+        return self.rotate.send_goal(goal)
+
+    def translate_x(self, goal):
+        return self.move_x.send_goal(goal)
+
+    def translate_coords(self, goal):
+        return self.move_coords.send_goal(goal)
+
+    def add(self, ws: WebSocket):
+        self.bt_action_client.addConnection(ws)
+        self.rotate.addConnection(ws)
+        self.move_coords.addConnection(ws)
+        self.move_x.addConnection(ws)
+        self.tf_web.addConnection(ws)
+
+    def remove(self, ws: WebSocket):
+        self.bt_action_client.removeConnection(ws)
+        self.rotate.removeConnection(ws)
+        self.move_coords.removeConnection(ws)
+        self.tf_web.removeConnection(ws)
+
+    def setRunner(server):
+        global actionClient
+        actionClient = server
+    
+    def getRunner():
+        global actionClient
+        return actionClient
+    
+
+
+
+    

toid_frontend/src/App.vue

@@ -1,57 +1,51 @@
 <script setup lang="ts">
-import RobotViewport from './components/RobotViewport.vue'
+import { computed } from 'vue'
+import { connectionStatus } from './stores/connection-status'
+import { ros } from './ts/ros_client'
+import { rosBackend } from './ts/robot_bridge'
+import { ROS2TFClient, Transform } from 'roslib'
+
+const store = connectionStatus()
+const connected = computed(() => {
+  return store.backendConnected && store.rosbridgeConnected
+})
+
+const reconnect = () => {
+  ros.manualConnect()
+  rosBackend.manualConnect()
+}
+
+const rosout = ros.ros.Topic({
+  name: '/rosout',
+  messageType: 'rcl_interfaces/msg/Log',
+})
+
+rosout.subscribe((msg) => {
+  console.log(msg)
+})
 </script>
 
 <template>
-  <div class="container">
+  <div>
-    <div class="row-container">
+    <div v-if="!connected" class="box-container">
-      <div class="box box1"></div>
+      Connecting...
-      <div class="box box2">
+      <button id="reconnect" @click="reconnect">Reconnect</button>
-        <RobotViewport />
+    </div>
-      </div>
+    <div class="box-container">
-      <div class="box box3"></div>
+      <h3>Current position:</h3>
     </div>
   </div>
 </template>
 
 <style scoped>
-.container {
+.box-container {
-  display: flex;
+  margin: 30px;
-  justify-content: stretch;
+  padding: 10px;
-  align-items: start;
+  border: 2px solid white;
-  width: 100vw;
-  height: calc(100vh - 1.5rem);
-  padding-top: 1.5rem;
 }
 
-.row-container {
+#reconnect {
-  width: 100vw;
+  margin-left: 2rem;
-  max-height: 100%;
+  background-color: green;
-  flex-shrink: 1;
-  display: flex;
-  margin: 0;
-  padding: 0;
-  justify-content: center;
-  flex-direction: row;
-}
-
-.box1 {
-  background-color: pink;
-  width: 3.5rem;
-}
-
-.box2 {
-  background-color: lightblue;
-  aspect-ratio: 3/2;
-  max-width: calc(150vh - 13.5rem);
-  max-height: calc((100vw - 13.5) * 2 / 3);
-  flex-grow: 2;
-}
-
-.box3 {
-  flex-grow: 1;
-  min-width: 10rem;
-  max-width: 20rem;
-  background-color: darkblue;
 }
 </style>

toid_frontend/src/assets/scss/global.scss

@@ -6,4 +6,5 @@ body,
   min-height: 100vh;
   max-width: 100vw;
   background-color: #242130;
+  color: #ffffff;
 }

toid_frontend/src/components/RobotControl.vue

@@ -0,0 +1,13 @@
+<script lang="ts"></script>
+
+<template>
+  <div>
+    <button>Send</button>
+    <label for="angle">Angle: </label>
+    <input type="text" name="angle" id="" />
+    <label for="angle">Min Angle: </label>
+    <input type="text" name="min_angle" id="" value="0" />
+  </div>
+</template>
+
+<style scoped></style>

toid_frontend/src/main.ts

@@ -3,34 +3,11 @@ import { createPinia } from 'pinia'
 
 import App from './App.vue'
 import router from './router'
-import { ros } from './ros_client'
-
 import '@/assets/scss/global.scss'
+import '@/ts/robot_bridge'
+import '@/ts/ros_client'
 
 const app = createApp(App)
-
 app.use(createPinia())
 app.use(router)
-
 app.mount('#app')
-
-ros.connect('http://localhost:3000')
-
-ros.on('error', (error) => {
-  console.log(error)
-  setTimeout(() => {
-    ros.connect('http://localhost:3000').catch()
-  }, 1000)
-})
-
-ros.on('close', () => {
-  setTimeout(() => {
-    if (!ros.isConnected) {
-      ros.connect('http://localhost:3000').catch()
-    }
-  }, 1000)
-})
-
-ros.on('connection', () => {
-  console.log('Connection made!')
-})

toid_frontend/src/ros_client.ts

@@ -1,9 +0,0 @@
-import * as ROSLIB from 'roslib'
-
-interface BehaviorTreeList {
-  tree_ids: Array<string>
-}
-
-const ros = new ROSLIB.Ros()
-
-export { ros, type BehaviorTreeList }

toid_frontend/src/stores/connection-status.ts

@@ -0,0 +1,8 @@
+import { ref } from 'vue'
+import { defineStore } from 'pinia'
+
+export const connectionStatus = defineStore('connstatus', () => {
+  const rosbridgeConnected = ref(false)
+  const backendConnected = ref(false)
+  return { rosbridgeConnected, backendConnected }
+})

toid_frontend/src/ts/robot_bridge.ts

@@ -0,0 +1,81 @@
+import { connectionStatus } from '@/stores/connection-status'
+
+class BackendBridge {
+  private static instance: BackendBridge
+  private readonly url: string = 'http://localhost:8000/action/ws'
+  private socket: WebSocket | null = null
+  private timeout: number | null = null
+
+  private reconnectAttempts = 0
+  private readonly maxAttempts = 10
+  private readonly baseDelay = 100
+
+  private constructor() {
+    this.connect()
+  }
+
+  public static getInstance(): BackendBridge {
+    if (!BackendBridge.instance) {
+      BackendBridge.instance = new BackendBridge()
+    }
+    return this.instance
+  }
+
+  public manualConnect() {
+    if (this.timeout) clearTimeout(this.timeout)
+    this.connect()
+  }
+
+  private connect(): void {
+    console.log(`Connecting to ${this.url}...`)
+    if (this.socket) {
+      connectionStatus().backendConnected = false
+      this.socket.onclose = null
+      this.socket.close()
+    }
+    this.socket = new WebSocket(this.url)
+
+    this.socket.onopen = () => {
+      connectionStatus().backendConnected = true
+      console.log('Connected successfully!')
+      this.reconnectAttempts = 0
+    }
+
+    this.socket.onclose = (event) => {
+      connectionStatus().backendConnected = false
+      console.warn('Socket closed:', event)
+      this.handleReconnect()
+    }
+
+    this.socket.onerror = (error) => {
+      console.error('Socket error:', error)
+    }
+  }
+
+  // Backoff algorithm
+  private handleReconnect(): void {
+    if (this.reconnectAttempts < this.maxAttempts) {
+      this.reconnectAttempts++
+
+      const delay = this.baseDelay * Math.pow(2, this.reconnectAttempts - 1)
+
+      console.log(
+        `Retrying in ${delay}ms... (Attempt ${this.reconnectAttempts}/${this.maxAttempts})`,
+      )
+
+      this.timeout = setTimeout(() => {
+        this.timeout = null
+        console.log(`Starting attempt [${this.reconnectAttempts}/${this.maxAttempts}]`)
+        this.connect()
+      }, delay)
+    } else {
+      console.error('Max reconnection attempts reached. Please check your connection.')
+    }
+  }
+}
+
+const rosBackend = BackendBridge.getInstance()
+
+export { rosBackend }
+
+export type { BackendBridge }

toid_frontend/src/ts/ros_client.ts

@@ -0,0 +1,84 @@
+import * as ROSLIB from 'roslib'
+import { connectionStatus } from '@/stores/connection-status'
+
+interface BehaviorTreeList {
+  tree_ids: Array<string>
+}
+
+class Ros {
+  private static instance: Ros
+  readonly ros: ROSLIB.Ros = new ROSLIB.Ros()
+  private readonly url: string = 'http://localhost:3000'
+  private timeout: number | null = null
+
+  private reconnectAttempts = 0
+  private readonly maxAttempts = 10
+  private readonly baseDelay = 100 // 1 second
+
+  private constructor() {
+    this.connect()
+  }
+
+  public static getInstance(): Ros {
+    if (!Ros.instance) {
+      Ros.instance = new Ros()
+    }
+    return Ros.instance
+  }
+
+  public reconnect() {
+    this.ros.connect(this.url)
+  }
+
+  public manualConnect() {
+    if (this.timeout) clearTimeout(this.timeout)
+    this.connect()
+  }
+
+  private connect(): void {
+    console.log(`Connecting to ${this.url}...`)
+
+    this.ros.on('connection', () => {
+      connectionStatus().rosbridgeConnected = true
+      console.log('Connected successfully!')
+      this.reconnectAttempts = 0
+    })
+
+    this.ros.on('close', (event: ROSLIB.TransportEvent) => {
+      connectionStatus().rosbridgeConnected = false
+      console.warn('Socket closed:', event)
+      this.handleReconnect()
+    })
+
+    this.ros.on('error', (error) => {
+      console.error('Socket error:', error)
+    })
+    this.ros.connect(this.url)
+  }
+
+  // Backoff algorithm
+  private handleReconnect(): void {
+    if (this.reconnectAttempts < this.maxAttempts) {
+      this.reconnectAttempts++
+
+      const delay = this.baseDelay * Math.pow(2, this.reconnectAttempts - 1)
+
+      console.log(
+        `Retrying in ${delay}ms... (Attempt ${this.reconnectAttempts}/${this.maxAttempts})`,
+      )
+
+      this.timeout = setTimeout(() => {
+        this.timeout = null
+        console.log(`Starting attempt [${this.reconnectAttempts}/${this.maxAttempts}]`)
+        this.ros.connect(this.url)
+      }, delay)
+    } else {
+      console.error('Max reconnection attempts reached. Please check your connection.')
+    }
+  }
+}
+
+// Usage:
+const ros = Ros.getInstance()
+
+export { ros, type BehaviorTreeList }