Initial webpage and backend

toid_frontend/src/App.vue

@@ -0,0 +1,57 @@
+<script setup lang="ts">
+import RobotViewport from './components/RobotViewport.vue'
+</script>
+
+<template>
+  <div class="container">
+    <div class="row-container">
+      <div class="box box1"></div>
+      <div class="box box2">
+        <RobotViewport />
+      </div>
+      <div class="box box3"></div>
+    </div>
+  </div>
+</template>
+
+<style scoped>
+.container {
+  display: flex;
+  justify-content: stretch;
+  align-items: start;
+  width: 100vw;
+  height: calc(100vh - 1.5rem);
+  padding-top: 1.5rem;
+}
+
+.row-container {
+  width: 100vw;
+  max-height: 100%;
+  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

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

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/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,36 @@
+import { createApp } from 'vue'
+import { createPinia } from 'pinia'
+
+import App from './App.vue'
+import router from './router'
+import { ros } from './ros_client'
+
+import '@/assets/scss/global.scss'
+
+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

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

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/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/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 }