cleanup old functions in frontend

.gitignore

@@ -1,4 +1,9 @@
 .cache
 build
 install
-log
+log
+
+node_modules
+dist
+
+__pycache__

scripts/idi.sh

@@ -25,9 +25,9 @@ source install/setup.bash
 case $TARGET in
     "na") 
         ros2 action send_goal /moveCoords toid_msgs/action/SimpleMoveCoords "
-          x: 0.0
-          y: 0.0
-          theta: 0.0
+          x: 1.325
+          y: 0.045
+          theta: -1.57079632679489661922
           backwards: ${SMER:-0}"
         ;;
     "pravo") 

toid_bt/launch/bt.launch.py

@@ -33,9 +33,24 @@ def generate_launch_description():
         parameters=[bt_params],
     )
 
+    rosbridge_ws = Node(
+        package='rosbridge_server',
+        executable='rosbridge_websocket',
+        output='screen',
+        arguments=["--port", "3000"],
+    )
+
+    tf2_web_publisher = Node(
+        package='tf2_web_republisher',
+        executable='tf2_web_republisher_node',
+        output='screen'
+    )
+
     return LaunchDescription([
         visualize_arg,
         use_mock_arg,
         bt_executor,
+        rosbridge_ws,
+        tf2_web_publisher,
     ])
 

toid_bt/package.xml

@@ -21,6 +21,9 @@
   <depend>toid_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>rosbridge_suite</depend>
+  <depend>tf2_web_republisher</depend>
+  <depend>tf2_web_republisher_interfaces</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

toid_cli/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>toid_cli</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="82343504+pimpest@users.noreply.github.com">petar</maintainer>
+  <license>MIT</license>
+
+  <depend>python3-fastapi</depend>
+  <depend>python3-pydantic</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

toid_cli/setup.cfg.old

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/toid_cli
+[install]
+install_scripts=$base/lib/toid_cli

toid_cli/setup.py

@@ -0,0 +1,25 @@
+from setuptools import find_packages, setup
+
+package_name = 'toid_cli'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='pimpest',
+    maintainer_email='82343504+pimpest@users.noreply.github.com',
+    description='TODO: Package description',
+    license='MIT',
+    entry_points={
+        'console_scripts': [
+            'toid_cli = toid_cli.main:main'
+        ],
+    },
+)

toid_cli/toid_cli/main.py

@@ -0,0 +1,70 @@
+from fastapi import FastAPI
+from pydantic import BaseModel
+from contextlib import asynccontextmanager
+from fastapi.middleware.cors import CORSMiddleware
+import asyncio
+import rclpy
+import uvicorn
+import threading
+import logging
+
+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=[
+        "*"
+    ],
+    allow_credentials=True,
+    allow_methods=["*"],
+    allow_headers=["*"],
+)
+
+
+
+app.include_router(startup.router, prefix="/startup")
+app.include_router(action.router, prefix="/action")
+# store running launch processes
+launch_processes = {}
+
+
+
+
+def main():
+    uvicorn.run(
+        "toid_cli.main:app",
+        host="0.0.0.0",
+        port=8000,
+        reload=False
+    )
+
+
+if __name__ == "__main__":
+    main()

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/.editorconfig

@@ -0,0 +1,8 @@
+[*.{js,jsx,mjs,cjs,ts,tsx,mts,cts,vue,css,scss,sass,less,styl}]
+charset = utf-8
+indent_size = 2
+indent_style = space
+insert_final_newline = true
+trim_trailing_whitespace = true
+end_of_line = lf
+max_line_length = 100

toid_frontend/.gitattributes

@@ -0,0 +1 @@
+* text=auto eol=lf

toid_frontend/.gitignore

@@ -0,0 +1,39 @@
+# Logs
+logs
+*.log
+npm-debug.log*
+yarn-debug.log*
+yarn-error.log*
+pnpm-debug.log*
+lerna-debug.log*
+
+node_modules
+.DS_Store
+dist
+dist-ssr
+coverage
+*.local
+
+# Editor directories and files
+.vscode/*
+!.vscode/extensions.json
+.idea
+*.suo
+*.ntvs*
+*.njsproj
+*.sln
+*.sw?
+
+*.tsbuildinfo
+
+.eslintcache
+
+# Cypress
+/cypress/videos/
+/cypress/screenshots/
+
+# Vitest
+__screenshots__/
+
+# Vite
+*.timestamp-*-*.mjs

toid_frontend/.oxlintrc.json

@@ -0,0 +1,10 @@
+{
+  "$schema": "./node_modules/oxlint/configuration_schema.json",
+  "plugins": ["eslint", "typescript", "unicorn", "oxc", "vue"],
+  "env": {
+    "browser": true
+  },
+  "categories": {
+    "correctness": "error"
+  }
+}

toid_frontend/.prettierrc.json

@@ -0,0 +1,6 @@
+{
+  "$schema": "https://json.schemastore.org/prettierrc",
+  "semi": false,
+  "singleQuote": true,
+  "printWidth": 100
+}

toid_frontend/.vscode/extensions.json

@@ -0,0 +1,9 @@
+{
+  "recommendations": [
+    "Vue.volar",
+    "dbaeumer.vscode-eslint",
+    "EditorConfig.EditorConfig",
+    "oxc.oxc-vscode",
+    "esbenp.prettier-vscode"
+  ]
+}

toid_frontend/README.md

@@ -0,0 +1,48 @@
+# .
+
+This template should help get you started developing with Vue 3 in Vite.
+
+## Recommended IDE Setup
+
+[VS Code](https://code.visualstudio.com/) + [Vue (Official)](https://marketplace.visualstudio.com/items?itemName=Vue.volar) (and disable Vetur).
+
+## Recommended Browser Setup
+
+- Chromium-based browsers (Chrome, Edge, Brave, etc.):
+  - [Vue.js devtools](https://chromewebstore.google.com/detail/vuejs-devtools/nhdogjmejiglipccpnnnanhbledajbpd)
+  - [Turn on Custom Object Formatter in Chrome DevTools](http://bit.ly/object-formatters)
+- Firefox:
+  - [Vue.js devtools](https://addons.mozilla.org/en-US/firefox/addon/vue-js-devtools/)
+  - [Turn on Custom Object Formatter in Firefox DevTools](https://fxdx.dev/firefox-devtools-custom-object-formatters/)
+
+## Type Support for `.vue` Imports in TS
+
+TypeScript cannot handle type information for `.vue` imports by default, so we replace the `tsc` CLI with `vue-tsc` for type checking. In editors, we need [Volar](https://marketplace.visualstudio.com/items?itemName=Vue.volar) to make the TypeScript language service aware of `.vue` types.
+
+## Customize configuration
+
+See [Vite Configuration Reference](https://vite.dev/config/).
+
+## Project Setup
+
+```sh
+npm install
+```
+
+### Compile and Hot-Reload for Development
+
+```sh
+npm run dev
+```
+
+### Type-Check, Compile and Minify for Production
+
+```sh
+npm run build
+```
+
+### Lint with [ESLint](https://eslint.org/)
+
+```sh
+npm run lint
+```

toid_frontend/env.d.ts

@@ -0,0 +1 @@
+/// <reference types="vite/client" />

toid_frontend/eslint.config.ts

@@ -0,0 +1,26 @@
+import { globalIgnores } from 'eslint/config'
+import { defineConfigWithVueTs, vueTsConfigs } from '@vue/eslint-config-typescript'
+import pluginVue from 'eslint-plugin-vue'
+import pluginOxlint from 'eslint-plugin-oxlint'
+import skipFormatting from 'eslint-config-prettier/flat'
+
+// To allow more languages other than `ts` in `.vue` files, uncomment the following lines:
+// import { configureVueProject } from '@vue/eslint-config-typescript'
+// configureVueProject({ scriptLangs: ['ts', 'tsx'] })
+// More info at https://github.com/vuejs/eslint-config-typescript/#advanced-setup
+
+export default defineConfigWithVueTs(
+  {
+    name: 'app/files-to-lint',
+    files: ['**/*.{vue,ts,mts,tsx}'],
+  },
+
+  globalIgnores(['**/dist/**', '**/dist-ssr/**', '**/coverage/**']),
+
+  ...pluginVue.configs['flat/essential'],
+  vueTsConfigs.recommended,
+
+  ...pluginOxlint.buildFromOxlintConfigFile('.oxlintrc.json'),
+
+  skipFormatting,
+)

toid_frontend/index.html

@@ -0,0 +1,13 @@
+<!DOCTYPE html>
+<html lang="">
+  <head>
+    <meta charset="UTF-8">
+    <link rel="icon" href="/favicon.ico">
+    <meta name="viewport" content="width=device-width, initial-scale=1.0">
+    <title>Vite App</title>
+  </head>
+  <body>
+    <div id="app"></div>
+    <script type="module" src="/src/main.ts"></script>
+  </body>
+</html>

toid_frontend/package.json

@@ -0,0 +1,48 @@
+{
+  "name": "toid_frontend",
+  "version": "0.0.0",
+  "private": true,
+  "type": "module",
+  "scripts": {
+    "dev": "vite",
+    "build": "run-p type-check \"build-only {@}\" --",
+    "preview": "vite preview",
+    "build-only": "vite build",
+    "type-check": "vue-tsc --build",
+    "lint": "run-s lint:*",
+    "lint:oxlint": "oxlint . --fix",
+    "lint:eslint": "eslint . --fix --cache",
+    "format": "prettier --write --experimental-cli src/"
+  },
+  "dependencies": {
+    "bootstrap": "^5.3.8",
+    "pinia": "^3.0.4",
+    "pixi-viewport": "^6.0.3",
+    "roslib": "^2.1.0",
+    "sass-embedded": "^1.97.3",
+    "vue": "^3.5.29",
+    "vue-router": "^5.0.3"
+  },
+  "devDependencies": {
+    "@tsconfig/node24": "^24.0.4",
+    "@types/node": "^24.11.0",
+    "@vitejs/plugin-vue": "^6.0.4",
+    "@vue/eslint-config-typescript": "^14.7.0",
+    "@vue/tsconfig": "^0.8.1",
+    "eslint": "^10.0.2",
+    "eslint-config-prettier": "^10.1.8",
+    "eslint-plugin-oxlint": "~1.50.0",
+    "eslint-plugin-vue": "~10.8.0",
+    "jiti": "^2.6.1",
+    "npm-run-all2": "^8.0.4",
+    "oxlint": "~1.50.0",
+    "prettier": "3.8.1",
+    "typescript": "~5.9.3",
+    "vite": "^7.3.1",
+    "vite-plugin-vue-devtools": "^8.0.6",
+    "vue-tsc": "^3.2.5"
+  },
+  "engines": {
+    "node": "^20.19.0 || >=22.12.0"
+  }
+}

toid_frontend/src/App.vue

@@ -0,0 +1,50 @@
+<script setup lang="ts">
+import { computed } from 'vue'
+import { connectionStatus } from './stores/connection-status'
+import { ros } from './ts/ros_client'
+import { rosBackend } from './ts/robot_bridge'
+
+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>
+    <div v-if="!connected" class="box-container">
+      Connecting...
+      <button id="reconnect" @click="reconnect">Reconnect</button>
+    </div>
+    <div class="box-container">
+      <h3>Current position:</h3>
+    </div>
+  </div>
+</template>
+
+<style scoped>
+.box-container {
+  margin: 30px;
+  padding: 10px;
+  border: 2px solid white;
+}
+
+#reconnect {
+  margin-left: 2rem;
+  background-color: green;
+}
+</style>

toid_frontend/src/assets/scss/global.scss

@@ -0,0 +1,10 @@
+//@import 'bootstrap/scss/bootstrap';
+
+body,
+#app {
+  margin: 0;
+  min-height: 100vh;
+  max-width: 100vw;
+  background-color: #242130;
+  color: #ffffff;
+}

toid_frontend/src/components/PageBanner.vue

@@ -0,0 +1,7 @@
+<script lang="ts"></script>
+
+<template>
+  <div>Banner</div>
+</template>
+
+<style scoped></style>

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/components/RobotViewport.vue

@@ -0,0 +1,116 @@
+<script setup lang="ts">
+import * as PIXI from 'pixi.js'
+import { Viewport } from 'pixi-viewport'
+import { onMounted, onBeforeUnmount, ref } from 'vue'
+import tableSvg from '@/assets/images/table.png'
+
+const viewportArea = ref<HTMLDivElement | null>(null)
+let app: PIXI.Application | null = null
+let viewport: Viewport | null = null
+let resizeObserver: ResizeObserver | null = null
+
+onMounted(async () => {
+  if (!viewportArea.value) return
+
+  app = new PIXI.Application()
+
+  // 1. Initialize with resizeTo
+  await app.init({
+    resizeTo: viewportArea.value,
+    antialias: true,
+    backgroundColor: 0x222222,
+    roundPixels: true,
+    resolution: window.devicePixelRatio * 2,
+    autoDensity: true,
+  })
+
+  viewportArea.value.appendChild(app.canvas)
+
+  // 2. Create Viewport
+  viewport = new Viewport({
+    screenWidth: viewportArea.value.clientWidth,
+    screenHeight: viewportArea.value.clientHeight,
+    worldWidth: 3000,
+    worldHeight: 2000,
+    events: app.renderer.events,
+  })
+
+  app.stage.addChild(viewport)
+
+  // 3. Configure Behaviors & Clamping
+  viewport
+    .drag()
+    .pinch()
+    .wheel()
+    .decelerate({
+      friction: 0.95,
+    })
+    .clamp({ direction: 'all' }) // Don't let user pan outside 1000x1000
+    .clampZoom({
+      maxHeight: 2000, // Max zoom out (show whole world)
+      minWidth: 500, // Max zoom in (detail)
+    })
+
+  // Add visual markers
+  setupScene(viewport)
+
+  // 4. Handle Dynamic Resizing
+  resizeObserver = new ResizeObserver(() => {
+    if (viewport && viewportArea.value) {
+      const { clientWidth, clientHeight } = viewportArea.value
+      // Tell the viewport the "window" size changed
+      viewport.resize(clientWidth, clientHeight)
+      viewport.fitWorld()
+    }
+  })
+  resizeObserver.observe(viewportArea.value)
+
+  viewportArea.value.addEventListener(
+    'wheel',
+    (e) => {
+      e.preventDefault()
+    },
+    { passive: false },
+  )
+})
+
+async function setupScene(v: Viewport) {
+  // Border
+
+  const svgTexture = await PIXI.Assets.load({
+    src: tableSvg,
+    data: {},
+  })
+
+  const background = new PIXI.Sprite({
+    texture: svgTexture,
+    width: v.worldWidth,
+    height: v.worldHeight,
+  })
+
+  v.addChild(background)
+
+  // Center Sprite
+  const sprite = v.addChild(new PIXI.Sprite(PIXI.Texture.WHITE))
+  sprite.tint = 0xff0000
+  sprite.setSize(100, 100)
+  sprite.position.set(450, 450)
+}
+
+onBeforeUnmount(() => {
+  resizeObserver?.disconnect()
+  app?.destroy(true, { children: true, texture: true })
+})
+</script>
+
+<template>
+  <div class="viewport-wrapper" ref="viewportArea"></div>
+</template>
+
+<style scoped>
+.viewport-wrapper {
+  overflow: hidden;
+  height: 100%;
+  aspect-ratio: 3/2;
+}
+</style>

toid_frontend/src/main.ts

@@ -0,0 +1,13 @@
+import { createApp } from 'vue'
+import { createPinia } from 'pinia'
+
+import App from './App.vue'
+import router from './router'
+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')

toid_frontend/src/router/index.ts

@@ -0,0 +1,8 @@
+import { createRouter, createWebHistory } from 'vue-router'
+
+const router = createRouter({
+  history: createWebHistory(import.meta.env.BASE_URL),
+  routes: [],
+})
+
+export default router

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/stores/counter.ts

@@ -0,0 +1,12 @@
+import { ref, computed } from 'vue'
+import { defineStore } from 'pinia'
+
+export const useCounterStore = defineStore('counter', () => {
+  const count = ref(0)
+  const doubleCount = computed(() => count.value * 2)
+  function increment() {
+    count.value++
+  }
+
+  return { count, doubleCount, increment }
+})

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 = 500 // 0.5 seconds
+
+  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,80 @@
+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 = 500 // 0.5 second
+
+  private constructor() {
+    this.connect()
+  }
+
+  public static getInstance(): Ros {
+    if (!Ros.instance) {
+      Ros.instance = new Ros()
+    }
+    return Ros.instance
+  }
+
+  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 }

toid_frontend/src/utils/smoothControlLaw.ts

@@ -0,0 +1,123 @@
+/**
+ * Utility functions to replicate NumPy behavior
+ */
+const clip = (val: number, min: number, max: number): number => Math.min(Math.max(val, min), max)
+
+const wrapToPi = (angle: number): number => Math.atan2(Math.sin(angle), Math.cos(angle))
+
+/**
+ * Calculates egocentric polar coordinates relative to a target
+ */
+const egocentricPolar = (
+  target: [number, number, number],
+  current: [number, number, number],
+  backward: boolean = false,
+): [number, number, number] => {
+  const [tx, ty, tyaw] = target
+  const [x, y, yaw] = current
+
+  const dx = tx - x
+  const dy = ty - y
+
+  const r = Math.hypot(dx, dy)
+
+  // Inverting dy for atan2 to match Python logic
+  let los = Math.atan2(-dy, dx)
+  if (backward) los += Math.PI
+
+  const phi = wrapToPi(tyaw + los)
+  const delta = wrapToPi(yaw + los)
+
+  return [r, phi, delta]
+}
+
+class SmoothControlLaw {
+  // Controller gains and constraints
+  public k_phi: number
+  public k_delta: number
+  public beta: number
+  public lam: number
+  public slowdown_radius: number
+  public v_linear_min: number
+  public v_linear_max: number
+  public v_angular_max: number
+
+  constructor(
+    k_phi: number = 3.0,
+    k_delta: number = 3.0,
+    beta: number = 0.3,
+    lam: number = 1.0,
+    slowdown_radius: number = 0.1,
+    v_linear_min: number = 0.1,
+    v_linear_max: number = 0.2,
+    v_angular_max: number = 2.0,
+  ) {
+    this.k_phi = k_phi
+    this.k_delta = k_delta
+    this.beta = beta
+    this.lam = lam
+    this.slowdown_radius = slowdown_radius
+    this.v_linear_min = v_linear_min
+    this.v_linear_max = v_linear_max
+    this.v_angular_max = v_angular_max
+  }
+
+  public curvature(r: number, phi: number, delta: number): number {
+    if (r < 1e-6) return 0.0
+
+    const prop = this.k_delta * (delta - Math.atan(-this.k_phi * phi))
+    const feedback = (1 + this.k_phi / (1 + Math.pow(this.k_phi * phi, 2))) * Math.sin(delta)
+
+    return (-1.0 / r) * (prop + feedback)
+  }
+
+  public velocity(
+    target: [number, number, number],
+    current: [number, number, number],
+    backward: boolean = false,
+  ): { v: number; w: number; kappa: number } {
+    const [r, phi, delta] = egocentricPolar(target, current, backward)
+
+    let kappa = this.curvature(r, phi, delta)
+    if (backward) kappa = -kappa
+
+    // Calculate linear velocity based on curvature
+    let v = this.v_linear_max / (1 + this.beta * Math.pow(Math.abs(kappa), this.lam))
+
+    // Slow down as we approach target
+    v = Math.min(this.v_linear_max * (r / this.slowdown_radius), v)
+    v = clip(v, this.v_linear_min, this.v_linear_max)
+
+    if (backward) v = -v
+
+    // Calculate angular velocity
+    let w = kappa * v
+    w = clip(w, -this.v_angular_max, this.v_angular_max)
+
+    // Adjust v if w was clipped to maintain the path curvature
+    if (Math.abs(kappa) > 1e-6) {
+      v = w / kappa
+    }
+
+    return { v, w, kappa }
+  }
+
+  public step(
+    target: [number, number, number],
+    current: [number, number, number],
+    dt: number,
+    backward: boolean = false,
+  ): [[number, number, number], number, number] {
+    const { v, w } = this.velocity(target, current, backward)
+
+    let [x, y, yaw] = current
+
+    x += v * dt * Math.cos(yaw)
+    y += v * dt * Math.sin(yaw)
+    yaw += w * dt
+
+    return [[x, y, yaw], v, w]
+  }
+}
+
+export { SmoothControlLaw }

toid_frontend/tsconfig.app.json

@@ -0,0 +1,18 @@
+{
+  "extends": "@vue/tsconfig/tsconfig.dom.json",
+  "include": ["env.d.ts", "src/**/*", "src/**/*.vue"],
+  "exclude": ["src/**/__tests__/*"],
+  "compilerOptions": {
+    // Extra safety for array and object lookups, but may have false positives.
+    "noUncheckedIndexedAccess": true,
+
+    // Path mapping for cleaner imports.
+    "paths": {
+      "@/*": ["./src/*"]
+    },
+
+    // `vue-tsc --build` produces a .tsbuildinfo file for incremental type-checking.
+    // Specified here to keep it out of the root directory.
+    "tsBuildInfoFile": "./node_modules/.tmp/tsconfig.app.tsbuildinfo"
+  }
+}

toid_frontend/tsconfig.json

@@ -0,0 +1,11 @@
+{
+  "files": [],
+  "references": [
+    {
+      "path": "./tsconfig.node.json"
+    },
+    {
+      "path": "./tsconfig.app.json"
+    }
+  ]
+}

toid_frontend/tsconfig.node.json

@@ -0,0 +1,28 @@
+// TSConfig for modules that run in Node.js environment via either transpilation or type-stripping.
+{
+  "extends": "@tsconfig/node24/tsconfig.json",
+  "include": [
+    "vite.config.*",
+    "vitest.config.*",
+    "cypress.config.*",
+    "nightwatch.conf.*",
+    "playwright.config.*",
+    "eslint.config.*"
+  ],
+  "compilerOptions": {
+    // Most tools use transpilation instead of Node.js's native type-stripping.
+    // Bundler mode provides a smoother developer experience.
+    "module": "preserve",
+    "moduleResolution": "bundler",
+
+    // Include Node.js types and avoid accidentally including other `@types/*` packages.
+    "types": ["node"],
+
+    // Disable emitting output during `vue-tsc --build`, which is used for type-checking only.
+    "noEmit": true,
+
+    // `vue-tsc --build` produces a .tsbuildinfo file for incremental type-checking.
+    // Specified here to keep it out of the root directory.
+    "tsBuildInfoFile": "./node_modules/.tmp/tsconfig.node.tsbuildinfo"
+  }
+}

toid_frontend/vite.config.ts

@@ -0,0 +1,18 @@
+import { fileURLToPath, URL } from 'node:url'
+
+import { defineConfig } from 'vite'
+import vue from '@vitejs/plugin-vue'
+import vueDevTools from 'vite-plugin-vue-devtools'
+
+// https://vite.dev/config/
+export default defineConfig({
+  plugins: [
+    vue(),
+    vueDevTools(),
+  ],
+  resolve: {
+    alias: {
+      '@': fileURLToPath(new URL('./src', import.meta.url))
+    },
+  },
+})