cleanup old functions in frontend

Added actions endpoint multiple clients can use to simultaneously control actions
Initial webpage and backend
2026-03-20 11:28:53 +01:00 · 2026-03-20 11:23:51 +01:00 · 2026-03-17 12:33:12 +01:00
124 changed files with 154067 additions and 4135 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,9 @@
 .cache
 build
 install
-log
+log
+
+node_modules
+dist
+
+__pycache__
--- a/docker-compose.yaml
+++ b/docker-compose.yaml
@@ -1,7 +1,6 @@
 services:
  toid:
    image: localhost:5000/toid 
-    build: .
    container_name: toid

    privileged: true
--- a/firmware/base/src/config.h
+++ b/firmware/base/src/config.h
@@ -24,8 +24,7 @@
 #define ENCODER_LEFT_PIN_B  13
 #define ENCODER_CPR         3840

-//#define WHEEL_RADIUS     (0.0300 * 1.01483541)
-#define WHEEL_RADIUS     0.028
-#define WHEEL_SEPARATION 0.271
+#define WHEEL_RADIUS     0.0300
+#define WHEEL_SEPARATION 0.264
 #define TIMER_CYCLE_US   1000
 //======================================================
--- a/firmware/base/src/main.c
+++ b/firmware/base/src/main.c
@@ -62,8 +62,8 @@ typedef struct calib_diff_t {
 } calib_diff_t;

 static calib_diff_t calib_enc = {
-    .left_gain = 1.0,  
-    .right_gain = 1.0,
+    .left_gain = 1.000,
+    .right_gain = 1.0000
 };

 void update_pos_cb() {
--- a/scripts/idi.sh
+++ b/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") 
--- a/toid_behaviors/include/toid_behaviors/move_coords.hpp
+++ b/toid_behaviors/include/toid_behaviors/move_coords.hpp
@@ -23,31 +23,23 @@ public:
    const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose & pose,
    const geometry_msgs::msg::Twist & vel) override;

-  double distanceToTarget(
-    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point, const double target_theta, bool backwards);
-
-  double velocityTarget(const double dist_left);
-  
-  bool collisionDetection(const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Pose &last_ok_pose);
-
  ResultStatus updateVel(
    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
    geometry_msgs::msg::Twist & out_vel) override;
  
  virtual nav2_core::CostmapInfoType getResourceInfo() override {
-    return nav2_core::CostmapInfoType::LOCAL;
+    return nav2_core::CostmapInfoType::NONE;
  }

 protected:

-  SmoothControlLaw scl_;
+  SmoothControlLaw scl;

  //Goal
  geometry_msgs::msg::Pose target_pose_;
  double target_angle_;
  double target_sign_;
  bool backwards_;
-  unsigned char mode_;

  //State
  double last_speed_;
--- a/toid_behaviors/include/toid_behaviors/scl.hpp
+++ b/toid_behaviors/include/toid_behaviors/scl.hpp
@@ -19,17 +19,13 @@ public:
  double v_angular_max = 2.0;

  void egocentric_polar(
-    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
-    bool backwards, double & r, double & phi, double & delta);
+    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
+    double & r, double & phi, double & delta);

  double curvature(double r, double phi, double delta);

  void calculate_vel(
-    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
+    const geometry_msgs::msg::Pose & target,const geometry_msgs::msg::Pose & current,
    geometry_msgs::msg::Twist & out_speed, bool backwards = false);
-
-  void step(
-    const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt,
-    bool backwards = false);
 };
 }  // namespace toid
--- a/toid_behaviors/include/toid_behaviors/simple_move.hpp
+++ b/toid_behaviors/include/toid_behaviors/simple_move.hpp
@@ -6,7 +6,6 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "toid_msgs/action/simple_rotate.hpp"
-#include "toid_msgs/msg/rival.hpp"

 namespace toid
 {
@@ -15,8 +14,6 @@ template <typename ActionT>
 class SimpleMove : public nav2_behaviors::TimedBehavior<ActionT>
 {
 public:
-  using Rival = toid_msgs::msg::Rival;
-
  virtual void configureCB() {}
  virtual void cleanupCB() {}

@@ -37,18 +34,6 @@ public:
    nav2_util::declare_parameter_if_not_declared(
      node, "odom_topic", rclcpp::ParameterValue("/odom"));
    std::string odom_topic_name = node->get_parameter("odom_topic").as_string();
-
-    nav2_util::declare_parameter_if_not_declared(node, "robot_width", rclcpp::ParameterValue(0.30));
-    node->get_parameter("robot_width", robot_width_);
-
-    nav2_util::declare_parameter_if_not_declared(
-      node, "robot_length", rclcpp::ParameterValue(0.30));
-    node->get_parameter("robot_length", robot_length_);
-
-    nav2_util::declare_parameter_if_not_declared(
-      node, "rival_radius", rclcpp::ParameterValue(0.30));
-    node->get_parameter("rival_radius", rival_radius_);
-
    odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
      odom_topic_name, 1, [&](nav_msgs::msg::Odometry msg) {
        std::lock_guard lock(mut_);
@@ -67,17 +52,12 @@ public:
  void activate() override
  {
    nav2_behaviors::TimedBehavior<ActionT>::activate();
-    rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock();
-    using namespace std::placeholders;
-    rivals_sub_ = node->create_subscription<Rival>(
-      "/dynamicObstacle", 1, std::bind(&SimpleMove<ActionT>::rival_cb, this, _1));
    activateCB();
  }

  void deactivate() override
  {
    nav2_behaviors::TimedBehavior<ActionT>::deactivate();
-    rivals_sub_.reset();
    deactivateCB();
  }

@@ -125,51 +105,12 @@ public:
    return r;
  }

-  bool check_rival_collision(geometry_msgs::msg::Pose2D & pose)
-  {
-    if (!rivals_) {
-      return false;
-    }
-    const double cosp = std::cos(pose.theta);
-    const double sinp = std::sin(pose.theta);
-    for (const auto & rival : rivals_->point) {
-      geometry_msgs::msg::Point local_rival;
-      const double dx = rival.x - pose.x;
-      const double dy = rival.y - pose.y;
-      local_rival.x = dx * cosp + dy * sinp;
-      local_rival.y = -dx * sinp + dy * cosp;
-
-      const double qx = std::abs(local_rival.x) - robot_length_ / 2.0;
-      const double qy = std::abs(local_rival.y) - robot_width_ / 2.0;
-
-      const double mqx = std::max(qx, 0.0);
-      const double mqy = std::max(qy, 0.0);
-
-      double length = std::sqrt(mqx * mqx + mqy * mqy);
-
-      double sdf = length + std::min(std::max(qx, qy), 0.0);
-      if (sdf < rival_radius_) {
-        return true;
-      }
-    }
-    return false;
-  }
-
-  void rival_cb(Rival::SharedPtr msg) { rivals_ = msg; }
-
 protected:
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
  geometry_msgs::msg::Pose current_pose_;
  geometry_msgs::msg::Twist current_vel_;
  std::recursive_mutex mut_;
  double control_duration_;
-
-  double robot_width_ = 0.30;
-  double robot_length_ = 0.30;
-  double rival_radius_ = 0.30;
-
-  Rival::SharedPtr rivals_;
-  rclcpp::Subscription<Rival>::SharedPtr rivals_sub_;
 };

 }  // namespace toid
--- a/toid_behaviors/include/toid_behaviors/simple_rotate.hpp
+++ b/toid_behaviors/include/toid_behaviors/simple_rotate.hpp
@@ -2,7 +2,6 @@

 #include "toid_behaviors/simple_move.hpp"
 #include "toid_msgs/action/simple_rotate.h"
-#include "toid_msgs/msg/rival.hpp"

 namespace toid
 {
@@ -24,25 +23,17 @@ public:
  ResultStatus updateVel(
    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
    geometry_msgs::msg::Twist & out_vel) override;
-
-  virtual nav2_core::CostmapInfoType getResourceInfo() override
-  {
-    return nav2_core::CostmapInfoType::LOCAL;
+  
+  virtual nav2_core::CostmapInfoType getResourceInfo() override {
+    return nav2_core::CostmapInfoType::NONE;
  }

-  void calc_err_and_sign(
-    double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign);
-
-  double check_space(const geometry_msgs::msg::Pose pose, const double e, const double sign);
-
-  double calc_speed(const double err, const double sign, const double angular_speed);
-
 protected:
+
  //Goal
  double target_angle_;
  double min_turn_angle_;
  double initial_direction_;
-  unsigned char mode_;

  //State
  double angular_speed_;
@@ -54,7 +45,6 @@ protected:
  double min_angular_speed_;
  double max_angular_accel_;
  double max_angular_decel_;
-  double lookahead_;
 };

 }  // namespace toid
--- a/toid_behaviors/src/move_coords.cpp
+++ b/toid_behaviors/src/move_coords.cpp
@@ -69,87 +69,43 @@ ResultStatus MoveCoords::onStart(
  target_sign_ = backwards_ ? -1.0 : 1.0;
  max_vel_speed_ = command->max_speed;

-  if (command->max_speed == 0) {
+  if(command->max_speed == 0) {
    auto node = node_.lock();
    node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
  }

-  mode_ = command->mode;
-
-  scl_.k_phi = k_phi_;
-  scl_.k_delta = k_delta_;
-  scl_.bbeta = beta_;
-  scl_.lam = lambda_;
-  scl_.slowdown_radius = slowdown_radius_;
-  scl_.v_angular_max = max_angular_speed_;
-  scl_.v_linear_min = min_vel_speed_;
-  scl_.v_linear_max = max_vel_speed_;
+  scl.k_phi = k_phi_;
+  scl.k_delta = k_delta_;
+  scl.bbeta = beta_;
+  scl.lam = lambda_;
+  scl.slowdown_radius = slowdown_radius_;
+  scl.v_angular_max = max_angular_speed_;
+  scl.v_linear_min = min_vel_speed_;
+  scl.v_linear_max = max_vel_speed_;

  last_speed_ = vel.angular.x;

  return ResultStatus{Status::SUCCEEDED};
 }

-double MoveCoords::distanceToTarget(
-  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point,
-  const double target_theta, bool backwards)
-{
-  const double dx = target_point.x - pose.position.x;
-  const double dy = target_point.y - pose.position.y;
-  const double target_sign = backwards ? -1.0 : 1.0;
-
-  return target_sign * (dx * cos(target_theta) + dy * sin(target_theta));
-}
-
-double MoveCoords::velocityTarget(const double dist_left)
-{
-  const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_;
-  const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_;
-
-  double vel = max_vel_speed_;
-  double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
-  vel = std::min(vel, max_vel_to_stop);
-  return std::clamp(target_sign_ * vel, lower_bound, upper_bound);
-}
-
-bool MoveCoords::collisionDetection(
-  const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose)
-{
-  const int samples = static_cast<int>(0.5 / control_duration_);
-  geometry_msgs::msg::Pose current_pose = pose;
-  last_ok_pose = pose;
-  const bool check_map = !(mode_ & MoveAction::Goal::IGNORE_OBSTACLES);
-  for (int i = 0; i < samples; i++) {
-    scl_.step(target_pose_, current_pose, control_duration_, backwards_);
-
-    geometry_msgs::msg::Pose2D p;
-    p.x = current_pose.position.x;
-    p.y = current_pose.position.y;
-    p.theta = tf2::getYaw(current_pose.orientation);
-
-    if (check_map && !local_collision_checker_->isCollisionFree(p, i == 0)) {
-      return true;
-    }
-
-    if (check_rival_collision(p)) {
-      return true;
-    }
-
-    last_ok_pose = current_pose;
-    const double dist_left =
-      distanceToTarget(current_pose, target_pose_.position, target_angle_, backwards_);
-    if (dist_left < 0.005) {
-      return false;
-    }
-  }
-  return false;
-}
-
 ResultStatus MoveCoords::updateVel(
  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
  geometry_msgs::msg::Twist & out_vel)
 {
-  double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_);
+  const double current_yaw = tf2::getYaw(pose.orientation);
+  double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
+
+  if (backwards_) {
+    angle_dist = angles::two_pi_complement(angle_dist);
+  }
+
+  const double dx = target_pose_.position.x - pose.position.x;
+  const double dy = target_pose_.position.y - pose.position.y;
+
+  const double dist_left = target_sign_ * (dx * cos(target_angle_) + dy * sin(target_angle_));
+
+  const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_;
+  const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_;

  if (dist_left <= 0.001) {
    out_vel.linear.x = 0;
@@ -157,48 +113,30 @@ ResultStatus MoveCoords::updateVel(
    return ResultStatus{Status::SUCCEEDED};
  }

-  const double current_yaw = tf2::getYaw(pose.orientation);
-  double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
+  double vel = max_vel_speed_;
+  double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
+  vel = std::min(vel, max_vel_to_stop);

-  geometry_msgs::msg::Pose last_ok_pose;
-  if (collisionDetection(pose, last_ok_pose)) {
-    dist_left = distanceToTarget(
-      pose, last_ok_pose.position, tf2::getYaw(last_ok_pose.orientation), backwards_);
-    if (dist_left <= 0.02) {
-      out_vel.linear.x = 0;
-      out_vel.angular.z = 0;
-    } else {
-      scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
-      scl_.calculate_vel(last_ok_pose, pose, out_vel, backwards_);
-    }
-
-    last_speed_ = out_vel.linear.x;
-
-    RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
-    RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max);
-
-    return ResultStatus{Status::RUNNING};
-  }

  if (dist_left <= 0.02) {
-    out_vel.linear.x = velocityTarget(dist_left);
-    out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
+    out_vel.linear.x = std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+    out_vel.angular.z = std::clamp(target_sign_ * kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
    last_speed_ = out_vel.linear.x;
    return ResultStatus{Status::RUNNING};
  }

-  scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
-  scl_.calculate_vel(target_pose_, pose, out_vel, backwards_);
-
+  scl.v_linear_max = target_sign_ * std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+  scl.calculate_vel(target_pose_, pose, out_vel, backwards_);
  last_speed_ = out_vel.linear.x;

-  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
-  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max);
+  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); 
+  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl.v_linear_max); 

  return ResultStatus{Status::RUNNING};
 }

 }  // namespace toid

+
 #include "pluginlib/class_list_macros.hpp"
 PLUGINLIB_EXPORT_CLASS(toid::MoveCoords, nav2_core::Behavior);
--- a/toid_behaviors/src/scl.cpp
+++ b/toid_behaviors/src/scl.cpp
@@ -3,21 +3,20 @@
 #include <cmath>

 #include "angles/angles.h"
-#include "nav2_util/geometry_utils.hpp"
 #include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

-namespace toid
-{
+
+namespace toid {
 void SmoothControlLaw::calculate_vel(
  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
-  geometry_msgs::msg::Twist & out_speed, bool backwards)
+  geometry_msgs::msg::Twist &out_speed, bool backwards)
 {
  double r, phi, delta;
  egocentric_polar(target, current, backwards, r, phi, delta);

  double curvature = this->curvature(r, phi, delta);
-  curvature = backwards ? -curvature : curvature;
+  curvature = backwards? -curvature : curvature;

  double v = v_linear_max / (1.0 + bbeta * std::pow(fabs(curvature), lam));

@@ -45,8 +44,8 @@ double SmoothControlLaw::curvature(double r, double phi, double delta)
 }

 void SmoothControlLaw::egocentric_polar(
-  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
-  double & r, double & phi, double & delta)
+  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards, double & r,
+  double & phi, double & delta)
 {
  const double dx = target.position.x - current.position.x;
  const double dy = target.position.y - current.position.y;
@@ -60,20 +59,4 @@ void SmoothControlLaw::egocentric_polar(
  delta = angles::normalize_angle(ctheta + los);
 }

-void SmoothControlLaw::step(
-  const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt,
-  bool backwards)
-{
-  geometry_msgs::msg::Twist twist;
-  calculate_vel(target, current, twist, backwards);
-
-  double theta = tf2::getYaw(current.orientation);
-  double dx = twist.linear.x * dt * cos(theta);
-  double dy = twist.linear.x * dt * sin(theta);
-  current.orientation =
-    nav2_util::geometry_utils::orientationAroundZAxis(theta + twist.angular.z * dt);
-  current.position.x += dx;
-  current.position.y += dy;
-}
-
-}  // namespace toid
+}
--- a/toid_behaviors/src/simple_rotate.cpp
+++ b/toid_behaviors/src/simple_rotate.cpp
@@ -4,7 +4,6 @@

 #include "angles/angles.h"
 #include "tf2/convert.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

 namespace toid
 {
@@ -31,10 +30,6 @@ void SimpleRotateBehavior::configureCB()
  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(4.0));
  node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_);
-
-  nav2_util::declare_parameter_if_not_declared(
-    node, behavior_name_ + ".lookahead", rclcpp::ParameterValue(0.5));
-  node->get_parameter(behavior_name_ + ".lookahead", lookahead_);
 }

 ResultStatus SimpleRotateBehavior::onStart(
@@ -45,19 +40,12 @@ ResultStatus SimpleRotateBehavior::onStart(
  min_turn_angle_ = abs(command->min_angle);
  initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0;
  max_angular_speed_ = command->max_speed;
-  mode_ = command->mode;

-  if (command->max_speed == 0) {
+  if(command->max_speed == 0) {
    auto node = node_.lock();
    node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
  }

-  geometry_msgs::msg::Pose2D pose2d;
-  pose2d.x = pose.position.x;
-  pose2d.y = pose.position.y;
-  pose2d.theta = initial_direction_;
-  local_collision_checker_->isCollisionFree(pose2d, true);
-
  last_angle_ = tf2::getYaw(pose.orientation);

  angular_speed_ = vel.angular.z;
@@ -65,108 +53,43 @@ ResultStatus SimpleRotateBehavior::onStart(
  return ResultStatus{Status::SUCCEEDED};
 }

-void SimpleRotateBehavior::calc_err_and_sign(
-  double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign)
-{
-  err = angles::shortest_angular_distance(current_yaw, target_angle_);
-  sign = (err < 0) ? -1.0 : 1.0;
-  err = std::abs(err);
-
-  if (min_turn_angle > 0.0) {
-    const double angle_change = angles::shortest_angular_distance(last_angle, current_yaw);
-    min_turn_angle = std::max(0.0, min_turn_angle - initial_direction_ * angle_change);
-    err = std::max(initial_direction_ * sign * err, 0.0);
-    err = std::max(min_turn_angle, err);
-    sign = initial_direction_;
-  }
-}
-
-double SimpleRotateBehavior::calc_speed(
-  const double err, const double sign, const double angular_speed)
-{
-  const double upper_vel_ = angular_speed + max_angular_accel_ * control_duration_;
-  const double lower_vel_ = angular_speed - max_angular_accel_ * control_duration_;
-
-  const double speed_until_overshoot = std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
-
-  const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
-  const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);
-  return speed;
-}
-
 ResultStatus SimpleRotateBehavior::updateVel(
  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
  geometry_msgs::msg::Twist & out_vel)
 {
  const double current_yaw = tf2::getYaw(pose.orientation);
-  double min_turn_angle = min_turn_angle_;
-  double angular_speed = angular_speed_;
-  double err, sign;
+  const double angle_change = angles::shortest_angular_distance(last_angle_ , current_yaw);
+  last_angle_ = current_yaw;

-  calc_err_and_sign(last_angle_, current_yaw, min_turn_angle, err, sign);
+  double err = angles::shortest_angular_distance(current_yaw, target_angle_);
+  double sign = (err < 0)? -1.0 : 1.0;
+  err = std::abs(err);

-  if (!(mode_ & RotateAction::Goal::IGNORE_OBSTACLES)) {
-    err = check_space(pose, err, sign);
+  if (min_turn_angle_ > 0.0) {
+    min_turn_angle_ = std::max(0.0, min_turn_angle_ - initial_direction_ * angle_change);
+    err = std::max( initial_direction_ * sign * err, 0.0);
+    err = std::max(min_turn_angle_, err);
+    sign = initial_direction_;
  }

-  double speed = 0.0;
+  const double upper_vel_ = angular_speed_ + max_angular_accel_ * control_duration_;
+  const double lower_vel_ = angular_speed_ - max_angular_accel_ * control_duration_;

-  if (err != 0.0) {
-    speed = calc_speed(err, sign, angular_speed);
-  }
+  const double speed_until_overshoot =
+    std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
+
+  const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
+  const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);

  if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.01) {
    return ResultStatus{Status::SUCCEEDED};
  }

-  min_turn_angle_ = min_turn_angle;
-  last_angle_ = current_yaw;
  angular_speed_ = speed;
  out_vel.angular.z = speed;
  return ResultStatus{Status::RUNNING};
 }

-double SimpleRotateBehavior::check_space(
-  const geometry_msgs::msg::Pose pose, const double e, const double sign)
-{
-  geometry_msgs::msg::Pose2D pose2d;
-  pose2d.x = pose.position.x;
-  pose2d.y = pose.position.y;
-  double initial_theta = tf2::getYaw(pose.orientation);
-  pose2d.theta = initial_theta;
-  const double step_size = 0.1;
-  const double err = std::min(e, 1.0);
-  const bool check_map = !(mode_ & RotateAction::Goal::IGNORE_OBSTACLES);
-
-  for (int i = 1; i < err / step_size; i++) {
-    pose2d.theta += sign * step_size;
-
-    if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
-      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
-      return step_size * (i - 1);
-    }
-
-    if (check_rival_collision(pose2d)) {
-      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
-      return step_size * (i - 1);
-    }
-  }
-
-  pose2d.theta = initial_theta + sign * err;
-
-  if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
-    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
-    return step_size * ((int)(err / step_size));
-  }
-
-  if (check_rival_collision(pose2d)) {
-    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
-    return step_size * ((int)(err / step_size));
-  }
-
-  return e;
-}
-
 }  // namespace toid

 #include "pluginlib/class_list_macros.hpp"
--- a/toid_bt/behavior_trees/behavior1.xml
+++ b/toid_bt/behavior_trees/behavior1.xml
@@ -1,111 +1,17 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
-  <BehaviorTree ID="seq">
-    <Sequence>
-      <Seq1 service_name=""/>
-      <Seq2 text="0101"
-            service_name=""/>
-      <Seq3 service_name=""/>
-    </Sequence>
-  </BehaviorTree>
-
-  <BehaviorTree ID="seq1">
-    <Sequence>
-      <ZeroOdom service_name=""/>
-      <MovePointSimple x="1.05"
-                       y="0"
-                       theta="0"
-                       max_speed="0.300000"
-                       backwards="false"
-                       action_name=""/>
-      <MovePointSimple x="0.76"
-                       y="0.18"
-                       theta="-90"
-                       max_speed="0.250000"
-                       backwards="true"
-                       action_name=""/>
-      <TranslateX x="0.5"
-                  max_speed="0.300000"
-                  action_name=""/>
-    </Sequence>
-  </BehaviorTree>
-
  <BehaviorTree ID="test_1">
    <Sequence>
-      <DetectStuck timeout="1.000000">
-        <RotateTowards x="0.4"
-                       y="0.0"
-                       max_speed="0.000000"
-                       min_angle="0.000000"
-                       action_name=""/>
-      </DetectStuck>
+      <RotateSimple angle="90"
+                    min_angle="0.000000"
+                    action_name=""/>
+      <Sleep msec="1000"/>
+      <RotateSimple angle="0"
+                    min_angle="0.000000"
+                    action_name=""/>
    </Sequence>
  </BehaviorTree>

  <!-- Description of Node Models (used by Groot) -->
-  <TreeNodesModel>
-    <Decorator ID="DetectStuck">
-      <input_port name="timeout"
-                  default="1.000000"
-                  type="double"/>
-    </Decorator>
-    <Action ID="MovePointSimple">
-      <input_port name="x"
-                  type="double"/>
-      <input_port name="y"
-                  type="double"/>
-      <input_port name="theta"
-                  type="double"/>
-      <input_port name="max_speed"
-                  default="0.000000"
-                  type="double"/>
-      <input_port name="backwards"
-                  default="false"
-                  type="bool"/>
-      <input_port name="action_name"
-                  type="std::string">Action server name</input_port>
-    </Action>
-    <Action ID="RotateTowards">
-      <input_port name="x"
-                  type="double"/>
-      <input_port name="y"
-                  type="double"/>
-      <input_port name="max_speed"
-                  default="0.000000"
-                  type="double"/>
-      <input_port name="min_angle"
-                  default="0.000000"
-                  type="double"/>
-      <input_port name="action_name"
-                  type="std::string">Action server name</input_port>
-    </Action>
-    <Action ID="Seq1">
-      <input_port name="service_name"
-                  type="std::string">Service name</input_port>
-    </Action>
-    <Action ID="Seq2">
-      <input_port name="text"
-                  type="std::string"/>
-      <input_port name="service_name"
-                  type="std::string">Service name</input_port>
-    </Action>
-    <Action ID="Seq3">
-      <input_port name="service_name"
-                  type="std::string">Service name</input_port>
-    </Action>
-    <Action ID="TranslateX">
-      <input_port name="x"
-                  type="double"/>
-      <input_port name="max_speed"
-                  default="0.000000"
-                  type="double"/>
-      <input_port name="action_name"
-                  type="std::string">Action server name</input_port>
-    </Action>
-    <Action ID="ZeroOdom">
-      <input_port name="service_name"
-                  type="std::string">Service name</input_port>
-    </Action>
-  </TreeNodesModel>

 </root>
--- a/toid_bt/behavior_trees/calibration_routines.xml
+++ b/toid_bt/behavior_trees/calibration_routines.xml
@@ -1,116 +1,45 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
-  <BehaviorTree ID="track_calib">
-    <Sequence>
-      <SetBlackboard value="0.265"
-                     output_key="width"/>
-      <Sleep msec="1000"/>
-      <ZeroOdom service_name=""/>
-      <RotateTowards x="0.4"
-                     y="0.0"
-                     max_speed="0.000000"
-                     min_angle="0.000000"
-                     action_name=""/>
-      <Sequence>
-        <Sleep msec="1000"/>
-        <MovePointSimple x="0.4"
-                         y="0"
-                         theta="0"
-                         max_speed="0.10000"
-                         backwards="false"
-                         action_name=""/>
-        <Sleep msec="500"/>
-        <RotateSimple angle="0"
-                      max_speed="0.500000"
-                      min_angle="270"
-                      action_name=""/>
-      </Sequence>
-      <ForceSuccess>
-        <DetectStuck timeout="1.000000">
-          <MovePointSimple x="-1.0"
-                           y="0"
-                           theta="0"
-                           max_speed="0.070000"
-                           backwards="true"
-                           action_name=""/>
-        </DetectStuck>
-      </ForceSuccess>
-      <SetWidth width="{width}"
-                count="1"
-                new_width="{width}"
-                service_name=""/>
-    </Sequence>
-  </BehaviorTree>
-
  <BehaviorTree ID="wheel_ratio_calibration">
    <Sequence>
-      <Sleep msec="5000"/>
-      <ZeroOdom service_name=""/>
-      <Sleep msec="1000"/>
+      <Delay delay_msec="5000">
+        <RotateSimple angle="0"
+                      max_speed="0.000000"
+                      min_angle="270"
+                      action_name=""/>
+      </Delay>
      <Sequence>
-        <MovePointSimple x="1.1"
+        <MovePointSimple x="1.0"
                         y="0"
                         theta="0"
                         max_speed="0.10000"
-                         backwards="false"
                         action_name=""/>
-        <Sleep msec="1000"/>
        <RotateSimple angle="180"
-                      max_speed="0.300000"
+                      max_speed="0.500000"
                      min_angle="10"
                      action_name=""/>
-        <Sleep msec="500"/>
-        <MovePointSimple x="0.35"
+        <MovePointSimple x="0.4"
                         y="0"
                         theta="180"
                         max_speed="0.100000"
-                         backwards="false"
                         action_name=""/>
        <RotateSimple angle="0"
-                      max_speed="0.300000"
+                      max_speed="0.500000"
                      min_angle="-10"
                      action_name=""/>
      </Sequence>
-      <ForceSuccess>
-        <DetectStuck timeout="1.000000">
-          <MovePointSimple x="-0.2"
-                           y="0"
-                           theta="0"
-                           max_speed="0.05"
-                           backwards="true"
-                           action_name=""/>
-        </DetectStuck>
-      </ForceSuccess>
-      <Sleep msec="1000"/>
-      <EndCalib service_name=""/>
-    </Sequence>
-  </BehaviorTree>
-
-  <BehaviorTree ID="wheel_size">
-    <Sequence>
-      <Sleep msec="1000"/>
-      <ZeroOdom service_name=""/>
-      <Sleep msec="1000"/>
-      <MovePointSimple x="1.200"
-                       y="0"
-                       theta="0"
-                       max_speed="0.250000"
-                       backwards="false"
-                       action_name=""/>
+      <TranslateX x="-0.5"
+                  max_speed="0.050000"
+                  action_name=""/>
+      <RotateSimple angle="0"
+                    max_speed="0.000000"
+                    min_angle="-270"
+                    action_name=""/>
    </Sequence>
  </BehaviorTree>

  <!-- Description of Node Models (used by Groot) -->
  <TreeNodesModel>
-    <Decorator ID="DetectStuck">
-      <input_port name="timeout"
-                  default="1.000000"
-                  type="double"/>
-    </Decorator>
-    <Action ID="EndCalib">
-      <input_port name="service_name"
-                  type="std::string">Service name</input_port>
-    </Action>
    <Action ID="MovePointSimple">
      <input_port name="x"
                  type="double"/>
@@ -121,9 +50,6 @@
      <input_port name="max_speed"
                  default="0.000000"
                  type="double"/>
-      <input_port name="backwards"
-                  default="false"
-                  type="bool"/>
      <input_port name="action_name"
                  type="std::string">Action server name</input_port>
    </Action>
@@ -139,35 +65,15 @@
      <input_port name="action_name"
                  type="std::string">Action server name</input_port>
    </Action>
-    <Action ID="RotateTowards">
+    <Action ID="TranslateX">
      <input_port name="x"
                  type="double"/>
-      <input_port name="y"
-                  type="double"/>
      <input_port name="max_speed"
                  default="0.000000"
                  type="double"/>
-      <input_port name="min_angle"
-                  default="0.000000"
-                  type="double"/>
      <input_port name="action_name"
                  type="std::string">Action server name</input_port>
    </Action>
-    <Action ID="SetWidth">
-      <input_port name="width"
-                  type="double"/>
-      <input_port name="count"
-                  default="1"
-                  type="int"/>
-      <output_port name="new_width"
-                   type="double"/>
-      <input_port name="service_name"
-                  type="std::string">Service name</input_port>
-    </Action>
-    <Action ID="ZeroOdom">
-      <input_port name="service_name"
-                  type="std::string">Service name</input_port>
-    </Action>
  </TreeNodesModel>

 </root>
--- a/toid_bt/behavior_trees/toid_behaviors.btproj
+++ b/toid_bt/behavior_trees/toid_behaviors.btproj
@@ -4,9 +4,6 @@
    <include path="calibration_routines.xml"/>
    <!-- Description of Node Models (used by Groot) -->
    <TreeNodesModel>
-        <Decorator ID="DetectStuck">
-            <input_port name="timeout" default="1.000000" type="double"/>
-        </Decorator>
        <Action ID="EndCalib">
            <input_port name="service_name" type="std::string">Service name</input_port>
        </Action>
@@ -15,7 +12,6 @@
            <input_port name="y" type="double"/>
            <input_port name="theta" type="double"/>
            <input_port name="max_speed" default="0.000000" type="double"/>
-            <input_port name="backwards" default="false" type="bool"/>
            <input_port name="action_name" type="std::string">Action server name</input_port>
        </Action>
        <Action ID="RotateSimple">
@@ -31,16 +27,6 @@
            <input_port name="min_angle" default="0.000000" type="double"/>
            <input_port name="action_name" type="std::string">Action server name</input_port>
        </Action>
-        <Action ID="Seq1">
-            <input_port name="service_name" type="std::string">Service name</input_port>
-        </Action>
-        <Action ID="Seq2">
-            <input_port name="text" type="std::string"/>
-            <input_port name="service_name" type="std::string">Service name</input_port>
-        </Action>
-        <Action ID="Seq3">
-            <input_port name="service_name" type="std::string">Service name</input_port>
-        </Action>
        <Action ID="SetWidth">
            <input_port name="width" type="double"/>
            <input_port name="count" default="1" type="int"/>
--- a/toid_bt/include/toid_bt/plugins/end_calib_action.hpp
+++ b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp
@@ -33,7 +33,7 @@ public:
  bool setRequest(typename Request::SharedPtr & request) override
  {
    auto width = getInput<double>("width").value();
-    double count = getInput<int>("count").value();
+    auto count = getInput<double>("count").value();

    geometry_msgs::msg::PoseStamped pose;
    get_pose(pose);
@@ -42,8 +42,6 @@ public:
    double new_width = width * (1 + (theta / (2 * M_PI * count)));
    request->data = new_width;

-    RCLCPP_INFO(logger(), "width is %lf", new_width);
-
    setOutput("new_width", new_width);
    return true;
  }
--- a/toid_bt/include/toid_bt/plugins/move_coords_action.hpp
+++ b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp
@@ -28,7 +28,6 @@ public:
      BT::InputPort<double>("y"),
      BT::InputPort<double>("theta"),
      BT::InputPort<double>("max_speed", 0, {}),
-      BT::InputPort<bool>("backwards", false, {}),
      //BT::InputPort<double>("options"),
    });
  }
@@ -39,7 +38,6 @@ public:
    auto y_goal = getInput<double>("y");
    auto theta = getInput<double>("theta");
    auto max_speed = getInput<double>("max_speed").value();
-    auto backwards = getInput<bool>("backwards").value();

    goal.x = x_goal.value();
    goal.y = y_goal.value();
@@ -52,8 +50,6 @@ public:
    }

    goal.max_speed = max_speed;
-    goal.backwards = backwards;
-    goal.mode = 1;

    return true;
  }
--- a/toid_bt/include/toid_bt/plugins/rotate_action.hpp
+++ b/toid_bt/include/toid_bt/plugins/rotate_action.hpp
@@ -40,7 +40,6 @@ public:
    goal.max_speed = max_speed.value();
    goal.angle = angles::from_degrees(goal.angle);
    goal.min_angle = angles::from_degrees(goal.min_angle);
-    goal.mode = 1;
    return true;
  }

--- a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp
+++ b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp
@@ -44,7 +44,6 @@ public:
    goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x);
    goal.min_angle = angles::from_degrees(min_angle);
    goal.max_speed = max_speed;
-    goal.mode = 1;

    return true;
  }
--- a/toid_bt/include/toid_bt/plugins/send_text_action.hpp
+++ b/toid_bt/include/toid_bt/plugins/send_text_action.hpp
@@ -1,43 +0,0 @@
-#pragma once
-
-#include "angles/angles.h"
-#include "behaviortree_ros2/bt_service_node.hpp"
-#include "tf2/utils.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-#include "toid_bt/plugin.hpp"
-
-#include "toid_msgs/srv/send_string.hpp"
-
-namespace toid
-{
-
-class SendTextNode : public BT::RosServiceNode<toid_msgs::srv::SendString>
-{
-public:
-  SendTextNode(
-    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
-  : BT::RosServiceNode<toid_msgs::srv::SendString>(name, conf, params)
-  {
-  }
-
-  static BT::PortsList providedPorts()
-  {
-    return providedBasicPorts({
-      BT::InputPort<std::string>("text"),
-    });
-  }
-
-  bool setRequest(typename Request::SharedPtr &req) override { 
-    std::string text = getInput<std::string>("text").value_or("");
-    req->text = text;
-
-    return true;
-  }
-
-  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
-  {
-    return BT::NodeStatus::SUCCESS;
-  }
-};
-
-}  // namespace toid
--- a/toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp
+++ b/toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp
@@ -1,69 +0,0 @@
-#pragma once
-
-#include "angles/angles.h"
-#include "behaviortree_ros2/bt_service_node.hpp"
-#include "std_srvs/srv/empty.hpp"
-#include "tf2/utils.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-#include "toid_bt/plugin.hpp"
-
-namespace toid
-{
-
-class StuckDetectorNode : public BT::DecoratorNode
-{
-public:
-  StuckDetectorNode(
-    const std::string & name, const BT::NodeConfig & conf, PoseFunc get_pose)
-  : BT::DecoratorNode(name, conf), get_pose(get_pose)
-  {
-  }
-
-  static BT::PortsList providedPorts() {
-    return {
-        BT::InputPort<double>("timeout", 1, {})
-    };
-  }
-
-  private:
-    bool checkIfPosesAreClose(geometry_msgs::msg::PoseStamped &poseA, geometry_msgs::msg::PoseStamped &poseB) {
-        const double dx = poseA.pose.position.x - poseB.pose.position.x;
-        const double dy = poseA.pose.position.y - poseB.pose.position.y;
-        const double dth = abs(tf2::getYaw(poseA.pose.orientation) - tf2::getYaw(poseB.pose.orientation));
-        return dx*dx + dy*dy < 0.004*0.004 && dth < 0.05;
-    }
-
-    BT::NodeStatus tick() override
-    {
-        if(status() == BT::NodeStatus::IDLE) {
-            setStatus(BT::NodeStatus::RUNNING);
-            last_pos_change = clock.now();
-        }
-
-        double timeout = getInput<double>("timeout").value_or(1.0);
-
-
-        geometry_msgs::msg::PoseStamped pose;
-        get_pose(pose);
-
-        if(checkIfPosesAreClose(last_pos, pose)) {
-            if(clock.now() - last_pos_change > rclcpp::Duration::from_seconds(timeout)) {
-                haltChild();
-                return BT::NodeStatus::FAILURE;
-            }
-        } else {
-            last_pos = pose;
-            last_pos_change = clock.now();
-        }
-
-        const BT::NodeStatus child_status = child_node_->executeTick();
-        return child_status;
-    }
-
-  geometry_msgs::msg::PoseStamped last_pos;
-  PoseFunc get_pose;
-  rclcpp::Time last_pos_change;
-  rclcpp::Clock clock;
-};
-
-}  // namespace toid
--- a/toid_bt/launch/bt.launch.py
+++ b/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,
    ])

--- a/toid_bt/package.xml
+++ b/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>
--- a/toid_bt/src/bt_executor.cpp
+++ b/toid_bt/src/bt_executor.cpp
@@ -9,8 +9,6 @@
 #include "toid_bt/plugins/move_coords_action.hpp"
 #include "toid_bt/plugins/rotate_action.hpp"
 #include "toid_bt/plugins/rotate_towards_action.hpp"
-#include "toid_bt/plugins/send_text_action.hpp"
-#include "toid_bt/plugins/stuck_detector_decorator.hpp"
 #include "toid_bt/plugins/translate_x_action.hpp"

 namespace toid
@@ -21,9 +19,9 @@ TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts)
  /* 
  executeRegistration();
  std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
-  */
  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  */

  nav2_util::declare_parameter_if_not_declared(
    node(), "base_frame", rclcpp::ParameterValue("base_footprint"));
@@ -62,16 +60,6 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)

  factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));

-  factory.registerNodeType<toid::EmptySrvNode>("Seq1", BT::RosNodeParams(nh, "/sequence1"));
-
-  BT::RosNodeParams service_params(nh, "/sequence2");
-  service_params.server_timeout = std::chrono::seconds(15);
-  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params);
-
-  factory.registerNodeType<toid::EmptySrvNode>("Seq3", BT::RosNodeParams(nh, "/sequence3"));
-
-  factory.registerNodeType<toid::StuckDetectorNode>("DetectStuck", get_pose);
-
  std::cout << describeCustomNodes() << std::endl;
 }

--- a/toid_interaction/package.xml
+++ b/toid_interaction/package.xml
@@ -1,18 +1,14 @@
 <?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_interaction</name>
+  <name>toid_cli</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
-  <maintainer email="82343504+pimpest@users.noreply.github.com">pimpest</maintainer>
+  <maintainer email="82343504+pimpest@users.noreply.github.com">petar</maintainer>
  <license>MIT</license>

-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
-
-  <depend>python3-serial</depend>
+  <depend>python3-fastapi</depend>
+  <depend>python3-pydantic</depend>

  <export>
    <build_type>ament_python</build_type>
--- a/toid_interaction/resource/toid_interaction
+++ b/toid_interaction/resource/toid_interaction
--- a/toid_cli/setup.cfg.old
+++ b/toid_cli/setup.cfg.old
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/toid_cli
+[install]
+install_scripts=$base/lib/toid_cli
--- a/toid_interaction/setup.py
+++ b/toid_interaction/setup.py
@@ -1,6 +1,6 @@
 from setuptools import find_packages, setup

-package_name = 'toid_interaction'
+package_name = 'toid_cli'

 setup(
    name=package_name,
@@ -17,15 +17,9 @@ setup(
    maintainer_email='82343504+pimpest@users.noreply.github.com',
    description='TODO: Package description',
    license='MIT',
-    extras_require={
-        'test': [
-            'pytest',
-        ],
-    },
    entry_points={
        'console_scripts': [
-            'sequence = toid_interaction.mechanism.sekvenca_2026:main',
-            'node = toid_interaction.interaction_node:main'
+            'toid_cli = toid_cli.main:main'
        ],
    },
 )
--- a/toid_interaction/toid_interaction/init.py
+++ b/toid_interaction/toid_interaction/init.py
--- a/toid_cli/toid_cli/main.py
+++ b/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()
--- a/toid_interaction/toid_interaction/mechanism/init.py
+++ b/toid_interaction/toid_interaction/mechanism/init.py
--- a/toid_cli/toid_cli/routers/action.py
+++ b/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")
--- a/toid_cli/toid_cli/routers/startup.py
+++ b/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) 
--- a/toid_cli/toid_cli/services/init.py
+++ b/toid_cli/toid_cli/services/init.py
--- a/toid_cli/toid_cli/services/runners.py
+++ b/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()
--- a/toid_cli/toid_cli/services/services.py
+++ b/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
+    
+
+
+
+    
--- a/toid_costmaps/CMakeLists.txt
+++ b/toid_costmaps/CMakeLists.txt
@@ -1,83 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(toid_costmaps)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-set(library_name toid_costmaps)
-
-set(
-  PACKAGE_DEPS
-  rclcpp
-  ament_index_cpp
-  Boost
-  geometry_msgs
-  pluginlib
-  nav_msgs
-  nav2_core
-  nav2_costmap_2d
-  nav2_util
-  tf2
-  tf2_geometry_msgs
-  tf2_ros
-  Eigen3
-  toid_msgs
-)
-
-set(
-  SOURCES
-  src/game_elements_layer.cpp
-  src/rival_layer.cpp
-)
-
-find_package(ament_cmake REQUIRED)
-foreach(PACKAGE ${PACKAGE_DEPS})
-  find_package(${PACKAGE} REQUIRED)
-endforeach()
-
-find_package(Boost REQUIRED COMPONENTS json)
-
-
-add_library(${library_name} SHARED ${SOURCES})
-
-target_link_libraries(
-  ${library_name}
-  Boost::json
-)
-
-target_include_directories(
-  ${library_name} 
-  PRIVATE 
-
-  include
-)
-
-ament_target_dependencies(
-  ${library_name}
-  ${PACKAGE_DEPS}
-)
-
-install(TARGETS ${library_name}
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin 
-)
-
-install(
-  DIRECTORY include/
-  DESTINATION include/
-)
-
-install(
-  DIRECTORY elements
-  DESTINATION share/${PROJECT_NAME}/
-)
-
-ament_export_include_directories(include)
-ament_export_libraries(${library_name})
-ament_export_dependencies(${PACKAGE_DEPS})
-
-pluginlib_export_plugin_description_file(nav2_costmap_2d toid_costmaps.xml)
-
-ament_package()
--- a/toid_costmaps/LICENSE
+++ b/toid_costmaps/LICENSE
@@ -1,202 +0,0 @@
-
-                                 Apache License
-                           Version 2.0, January 2004
-                        http://www.apache.org/licenses/
-
-   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
-
-   1. Definitions.
-
-      "License" shall mean the terms and conditions for use, reproduction,
-      and distribution as defined by Sections 1 through 9 of this document.
-
-      "Licensor" shall mean the copyright owner or entity authorized by
-      the copyright owner that is granting the License.
-
-      "Legal Entity" shall mean the union of the acting entity and all
-      other entities that control, are controlled by, or are under common
-      control with that entity. For the purposes of this definition,
-      "control" means (i) the power, direct or indirect, to cause the
-      direction or management of such entity, whether by contract or
-      otherwise, or (ii) ownership of fifty percent (50%) or more of the
-      outstanding shares, or (iii) beneficial ownership of such entity.
-
-      "You" (or "Your") shall mean an individual or Legal Entity
-      exercising permissions granted by this License.
-
-      "Source" form shall mean the preferred form for making modifications,
-      including but not limited to software source code, documentation
-      source, and configuration files.
-
-      "Object" form shall mean any form resulting from mechanical
-      transformation or translation of a Source form, including but
-      not limited to compiled object code, generated documentation,
-      and conversions to other media types.
-
-      "Work" shall mean the work of authorship, whether in Source or
-      Object form, made available under the License, as indicated by a
-      copyright notice that is included in or attached to the work
-      (an example is provided in the Appendix below).
-
-      "Derivative Works" shall mean any work, whether in Source or Object
-      form, that is based on (or derived from) the Work and for which the
-      editorial revisions, annotations, elaborations, or other modifications
-      represent, as a whole, an original work of authorship. For the purposes
-      of this License, Derivative Works shall not include works that remain
-      separable from, or merely link (or bind by name) to the interfaces of,
-      the Work and Derivative Works thereof.
-
-      "Contribution" shall mean any work of authorship, including
-      the original version of the Work and any modifications or additions
-      to that Work or Derivative Works thereof, that is intentionally
-      submitted to Licensor for inclusion in the Work by the copyright owner
-      or by an individual or Legal Entity authorized to submit on behalf of
-      the copyright owner. For the purposes of this definition, "submitted"
-      means any form of electronic, verbal, or written communication sent
-      to the Licensor or its representatives, including but not limited to
-      communication on electronic mailing lists, source code control systems,
-      and issue tracking systems that are managed by, or on behalf of, the
-      Licensor for the purpose of discussing and improving the Work, but
-      excluding communication that is conspicuously marked or otherwise
-      designated in writing by the copyright owner as "Not a Contribution."
-
-      "Contributor" shall mean Licensor and any individual or Legal Entity
-      on behalf of whom a Contribution has been received by Licensor and
-      subsequently incorporated within the Work.
-
-   2. Grant of Copyright License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      copyright license to reproduce, prepare Derivative Works of,
-      publicly display, publicly perform, sublicense, and distribute the
-      Work and such Derivative Works in Source or Object form.
-
-   3. Grant of Patent License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      (except as stated in this section) patent license to make, have made,
-      use, offer to sell, sell, import, and otherwise transfer the Work,
-      where such license applies only to those patent claims licensable
-      by such Contributor that are necessarily infringed by their
-      Contribution(s) alone or by combination of their Contribution(s)
-      with the Work to which such Contribution(s) was submitted. If You
-      institute patent litigation against any entity (including a
-      cross-claim or counterclaim in a lawsuit) alleging that the Work
-      or a Contribution incorporated within the Work constitutes direct
-      or contributory patent infringement, then any patent licenses
-      granted to You under this License for that Work shall terminate
-      as of the date such litigation is filed.
-
-   4. Redistribution. You may reproduce and distribute copies of the
-      Work or Derivative Works thereof in any medium, with or without
-      modifications, and in Source or Object form, provided that You
-      meet the following conditions:
-
-      (a) You must give any other recipients of the Work or
-          Derivative Works a copy of this License; and
-
-      (b) You must cause any modified files to carry prominent notices
-          stating that You changed the files; and
-
-      (c) You must retain, in the Source form of any Derivative Works
-          that You distribute, all copyright, patent, trademark, and
-          attribution notices from the Source form of the Work,
-          excluding those notices that do not pertain to any part of
-          the Derivative Works; and
-
-      (d) If the Work includes a "NOTICE" text file as part of its
-          distribution, then any Derivative Works that You distribute must
-          include a readable copy of the attribution notices contained
-          within such NOTICE file, excluding those notices that do not
-          pertain to any part of the Derivative Works, in at least one
-          of the following places: within a NOTICE text file distributed
-          as part of the Derivative Works; within the Source form or
-          documentation, if provided along with the Derivative Works; or,
-          within a display generated by the Derivative Works, if and
-          wherever such third-party notices normally appear. The contents
-          of the NOTICE file are for informational purposes only and
-          do not modify the License. You may add Your own attribution
-          notices within Derivative Works that You distribute, alongside
-          or as an addendum to the NOTICE text from the Work, provided
-          that such additional attribution notices cannot be construed
-          as modifying the License.
-
-      You may add Your own copyright statement to Your modifications and
-      may provide additional or different license terms and conditions
-      for use, reproduction, or distribution of Your modifications, or
-      for any such Derivative Works as a whole, provided Your use,
-      reproduction, and distribution of the Work otherwise complies with
-      the conditions stated in this License.
-
-   5. Submission of Contributions. Unless You explicitly state otherwise,
-      any Contribution intentionally submitted for inclusion in the Work
-      by You to the Licensor shall be under the terms and conditions of
-      this License, without any additional terms or conditions.
-      Notwithstanding the above, nothing herein shall supersede or modify
-      the terms of any separate license agreement you may have executed
-      with Licensor regarding such Contributions.
-
-   6. Trademarks. This License does not grant permission to use the trade
-      names, trademarks, service marks, or product names of the Licensor,
-      except as required for reasonable and customary use in describing the
-      origin of the Work and reproducing the content of the NOTICE file.
-
-   7. Disclaimer of Warranty. Unless required by applicable law or
-      agreed to in writing, Licensor provides the Work (and each
-      Contributor provides its Contributions) on an "AS IS" BASIS,
-      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
-      implied, including, without limitation, any warranties or conditions
-      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
-      PARTICULAR PURPOSE. You are solely responsible for determining the
-      appropriateness of using or redistributing the Work and assume any
-      risks associated with Your exercise of permissions under this License.
-
-   8. Limitation of Liability. In no event and under no legal theory,
-      whether in tort (including negligence), contract, or otherwise,
-      unless required by applicable law (such as deliberate and grossly
-      negligent acts) or agreed to in writing, shall any Contributor be
-      liable to You for damages, including any direct, indirect, special,
-      incidental, or consequential damages of any character arising as a
-      result of this License or out of the use or inability to use the
-      Work (including but not limited to damages for loss of goodwill,
-      work stoppage, computer failure or malfunction, or any and all
-      other commercial damages or losses), even if such Contributor
-      has been advised of the possibility of such damages.
-
-   9. Accepting Warranty or Additional Liability. While redistributing
-      the Work or Derivative Works thereof, You may choose to offer,
-      and charge a fee for, acceptance of support, warranty, indemnity,
-      or other liability obligations and/or rights consistent with this
-      License. However, in accepting such obligations, You may act only
-      on Your own behalf and on Your sole responsibility, not on behalf
-      of any other Contributor, and only if You agree to indemnify,
-      defend, and hold each Contributor harmless for any liability
-      incurred by, or claims asserted against, such Contributor by reason
-      of your accepting any such warranty or additional liability.
-
-   END OF TERMS AND CONDITIONS
-
-   APPENDIX: How to apply the Apache License to your work.
-
-      To apply the Apache License to your work, attach the following
-      boilerplate notice, with the fields enclosed by brackets "[]"
-      replaced with your own identifying information. (Don't include
-      the brackets!)  The text should be enclosed in the appropriate
-      comment syntax for the file format. We also recommend that a
-      file or class name and description of purpose be included on the
-      same "printed page" as the copyright notice for easier
-      identification within third-party archives.
-
-   Copyright [yyyy] [name of copyright owner]
-
-   Licensed under the Apache License, Version 2.0 (the "License");
-   you may not use this file except in compliance with the License.
-   You may obtain a copy of the License at
-
-       http://www.apache.org/licenses/LICENSE-2.0
-
-   Unless required by applicable law or agreed to in writing, software
-   distributed under the License is distributed on an "AS IS" BASIS,
-   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-   See the License for the specific language governing permissions and
-   limitations under the License.
--- a/toid_costmaps/elements/elements.json
+++ b/toid_costmaps/elements/elements.json
@@ -1,46 +0,0 @@
-{
-  "game_elements": [
-    {
-      "x": 0.0,
-      "y": 0.7,
-      "width": 0.2,
-      "height": 0.2
-    },
-    {
-      "x": 0.05,
-      "y": 0.3,
-      "width": 0.2,
-      "height": 0.2
-    },
-    {
-      "x": 0.6,
-      "y": 0.0,
-      "width": 0.2,
-      "height": 0.2
-    },
-    {
-      "x": 0.05,
-      "y": 1.1,
-      "width": 0.2,
-      "height": 0.2
-    }
-  ],
-
-  "blue": [
-    {
-      "x": 0.00,
-      "y": 1.55,
-      "width": 0.6,
-      "height": 0.45
-    }
-  ],
-
-  "yellow": [
-    {
-      "x": 2.40,
-      "y": 1.55,
-      "width": 0.6,
-      "height": 0.45
-    }
-  ]
-}
--- a/toid_costmaps/include/toid_costmaps/element_info.hpp
+++ b/toid_costmaps/include/toid_costmaps/element_info.hpp
@@ -1,46 +0,0 @@
-#pragma once
-
-#include "boost/json.hpp"
-
-namespace toid
-{
-class GameElement
-{
-public:
-  GameElement(double x, double y, double width, double height)
-  : x_(x), y_(y), width_(width), height_(height)
-  {
-  }
-
-  void start(double & x, double & y) const
-  {
-    x = this->x_;
-    y = this->y_;
-  }
-
-  void end(double & x, double & y) const
-  {
-    x = this->x_ + this->width_;
-    y = this->y_ + this->height_;
-  }
-
-private:
-  double x_;
-  double y_;
-  double width_;
-  double height_;
-};
-
-inline GameElement tag_invoke(
-  boost::json::value_to_tag<GameElement>, boost::json::value const & jv)
-{
-    auto const& obj = jv.as_object();
-    return GameElement{
-        boost::json::value_to<double>(obj.at("x")) - 1.5,
-        boost::json::value_to<double>(obj.at("y")) - 1.0,
-        boost::json::value_to<double>(obj.at("width")),
-        boost::json::value_to<double>(obj.at("height"))
-    };
-}
-
-}  // namespace toid
--- a/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp
+++ b/toid_costmaps/include/toid_costmaps/game_elements_layer.hpp
@@ -1,58 +0,0 @@
-#pragma once
-
-#include "jsoncpp/json/json.h"
-#include "nav2_costmap_2d/costmap_layer.hpp"
-#include "nav2_costmap_2d/layer.hpp"
-#include "nav2_costmap_2d/layered_costmap.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "toid_costmaps/element_info.hpp"
-#include "toid_msgs/msg/active_elements.hpp"
-
-namespace toid
-{
-
-class GameElementLayer : public nav2_costmap_2d::CostmapLayer
-{
-  using ActiveElements = toid_msgs::msg::ActiveElements;
-
-public:
-  GameElementLayer() { costmap_ = NULL; }
-  ~GameElementLayer() {}
-
-  void onInitialize() override;
-  void activate() override;
-  void deactivate() override;
-
-  void updateBounds(
-    double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y,
-    double * max_x, double * max_y) override;
-
-  void updateCosts(
-    nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) override;
-
-  void reset() override { return; }
-
-  void onFootprintChanged() override {}
-
-  void placeElement(nav2_costmap_2d::Costmap2D & grid, const GameElement & element);
-
-  bool isClearable() override { return true; }
-
-  void initGameElements();
-
-  void active_elements_cb(ActiveElements::SharedPtr msg);
-
-private:
-  double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
-
-  bool need_recalculation_;
-
-  std::vector<GameElement> game_elements_;
-  std::vector<GameElement> static_elements_;
-  std::vector<std::string> extra_elements_;
-  ActiveElements::SharedPtr active_elements_;
-
-  rclcpp::Subscription<ActiveElements>::SharedPtr active_elements_sub_;
-};
-
-}  // namespace toid
--- a/toid_costmaps/include/toid_costmaps/rival_layer.hpp
+++ b/toid_costmaps/include/toid_costmaps/rival_layer.hpp
@@ -1,55 +0,0 @@
-#pragma once
-
-#include "nav2_costmap_2d/costmap_layer.hpp"
-#include "nav2_costmap_2d/layered_costmap.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "toid_msgs/msg/rival.hpp"
-
-
-namespace toid
-{
-
-class RivalLayer : public nav2_costmap_2d::CostmapLayer
-{
-  using Rivals = toid_msgs::msg::Rival;
-
-public:
-  RivalLayer() { costmap_ = NULL; }
-  ~RivalLayer() {}
-
-  void onInitialize() override;
-  void activate() override;
-  void deactivate() override;
-
-  void updateBounds(
-    double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y,
-    double * max_x, double * max_y) override;
-
-  void updateCosts(
-    nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) override;
-
-  void reset() override { return; }
-
-  void onFootprintChanged() override {}
-
-  void placeRival(nav2_costmap_2d::Costmap2D & grid, double x, double y);
-
-  bool isClearable() override { return true; }
-
-  void rival_cb(Rivals::SharedPtr msg);
-
-private:
-  double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
-
-  bool need_recalculation_;
-
-  uint debounce_ = 0;
-
-  Rivals::SharedPtr rivals_;
-
-  rclcpp::Subscription<Rivals>::SharedPtr rival_sub_;
-
-  double rival_size_ = 0.15;
-};
-
-}  // namespace toid
--- a/toid_costmaps/package.xml
+++ b/toid_costmaps/package.xml
@@ -1,32 +0,0 @@
-<?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_costmaps</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="82343504+pimpest@users.noreply.github.com">Pimpest</maintainer>
-  <license>Apache-2.0</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>angles</depend>
-  <depend>ament_index_cpp</depend>
-  <depend>boost</depend>
-  <depend>geometry_msgs</depend>
-  <depend>nav_msgs</depend>
-  <depend>nav2_costmap_2d</depend>
-  <depend>nav2_util</depend>
-  <depend>nav2_core</depend>
-  <depend>pluginlib</depend>
-  <depend>tf2</depend>
-  <depend>tf2_geometry_msgs</depend>
-  <depend>tf2_ros</depend>
-  <depend>toid_msgs</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
--- a/toid_costmaps/src/game_elements_layer.cpp
+++ b/toid_costmaps/src/game_elements_layer.cpp
@@ -1,138 +0,0 @@
-#include "toid_costmaps/game_elements_layer.hpp"
-
-#include <fstream>
-
-#include "ament_index_cpp/get_package_share_directory.hpp"
-#include "nav2_util/node_utils.hpp"
-
-namespace toid
-{
-
-void GameElementLayer::onInitialize()
-{
-  auto node = node_.lock();
-  if (!node) {
-    return;
-  }
-}
-
-void GameElementLayer::activate()
-{
-  auto node = node_.lock();
-
-  nav2_util::declare_parameter_if_not_declared(
-    node, name_ + ".extra_elements", rclcpp::ParameterValue(std::vector<std::string>{}));
-  node->get_parameter(name_ + ".extra_elements", extra_elements_);
-
-  initGameElements();
-
-  active_elements_ = std::make_unique<ActiveElements>();
-  active_elements_->active = std::vector<bool>(game_elements_.size(), true);
-
-  using namespace std::placeholders;
-  active_elements_sub_ = node->create_subscription<ActiveElements>(
-    "/active_elements", 1, std::bind(&GameElementLayer::active_elements_cb, this, _1));
-}
-
-void GameElementLayer::deactivate()
-{
-  auto node = node_.lock();
-  active_elements_sub_.reset();
-  active_elements_.reset();
-  game_elements_.clear();
-  static_elements_.clear();
-  extra_elements_.clear();
-}
-
-void GameElementLayer::active_elements_cb(ActiveElements::SharedPtr msg)
-{
-  active_elements_ = msg;
-}
-
-void GameElementLayer::updateBounds(
-  double, double, double, double * min_x, double * min_y, double * max_x, double * max_y)
-{
-  touch(1.50, 1.0, min_x, min_y, max_x, max_y);
-  touch(-1.50, -1.0, min_x, min_y, max_x, max_y);
-}
-
-void GameElementLayer::updateCosts(nav2_costmap_2d::Costmap2D & grid, int, int, int, int)
-{
-  ulong idx = 0;
-  for (const auto & elem : game_elements_) {
-    if (active_elements_->active.size() > idx && active_elements_->active.at(idx)) {
-      placeElement(grid, elem);
-    }
-    idx++;
-  }
-
-  for (const auto & elem : static_elements_) {
-    placeElement(grid, elem);
-  }
-}
-
-void GameElementLayer::placeElement(nav2_costmap_2d::Costmap2D & grid, const GameElement & element)
-{
-  bool in_bounds = true;
-  double ex, ey;
-  element.start(ex, ey);
-  uint mx, my;
-  in_bounds &= worldToMap(ex + 0.001, ey + 0.001, mx, my);
-  element.end(ex, ey);
-  uint mmx, mmy;
-  in_bounds &= worldToMap(ex + 0.001, ey + 0.001, mmx, mmy);
-
-  if (in_bounds) {
-    for (uint j = my; j < mmy; j++) {
-      for (uint i = mx; i < mmx; i++) {
-        grid.setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE);
-      }
-    }
-  }
-}
-
-void GameElementLayer::initGameElements()
-{
-  std::string file_path = "/elements/elements.json";
-  std::string base_path = ament_index_cpp::get_package_share_directory("toid_costmaps");
-
-  RCLCPP_INFO(rclcpp::get_logger("toid_costmap"), "Bro hear me out");
-
-  std::ifstream fs(base_path + file_path);
-  if (!fs.is_open()) {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("toid_costmap"), "Failed to open elements file: %s%s", base_path.c_str(),
-      file_path.c_str());
-    throw std::runtime_error("File not found");
-  }
-
-  boost::json::stream_parser p;
-  boost::json::error_code ec;
-
-  char buffer[4096];
-  while (fs.read(buffer, sizeof(buffer)) || fs.gcount() > 0) {
-    p.write(buffer, fs.gcount(), ec);
-    if (ec) {
-      RCLCPP_FATAL(rclcpp::get_logger("toid_costmap"), "JsonError: %s", ec.message().c_str());
-      throw std::runtime_error("JsonError");
-    }
-  }
-  p.finish(ec);
-  if (ec) {
-    RCLCPP_FATAL(rclcpp::get_logger("toid_costmap"), "JsonError: %s", ec.message().c_str());
-    throw std::runtime_error("JsonError");
-  }
-
-  boost::json::value jv = p.release();
-
-  game_elements_ = boost::json::value_to<std::vector<GameElement>>(jv.at("game_elements"));
-  for (const auto & t : extra_elements_) {
-    std::vector<GameElement> elements = boost::json::value_to<std::vector<GameElement>>(jv.at(t));
-    static_elements_.insert(static_elements_.end(), elements.begin(), elements.end());
-  }
-}
-
-}  // namespace toid
-
-#include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(toid::GameElementLayer, nav2_costmap_2d::Layer);
--- a/toid_costmaps/src/rival_layer.cpp
+++ b/toid_costmaps/src/rival_layer.cpp
@@ -1,108 +0,0 @@
-
-#include "toid_costmaps/rival_layer.hpp"
-
-#include "geometry_msgs/msg/transform_stamped.hpp"
-#include "nav2_util/node_utils.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-
-namespace toid
-{
-
-void RivalLayer::onInitialize()
-{
-  auto node = node_.lock();
-  if (!node) {
-    return;
-  }
-}
-
-void RivalLayer::activate()
-{
-  auto node = node_.lock();
-
-  nav2_util::declare_parameter_if_not_declared(
-    node, name_ + ".rival_size", rclcpp::ParameterValue(0.15));
-  node->get_parameter(name_ + ".rival_size", rival_size_);
-
-  using namespace std::placeholders;
-  rival_sub_ = node->create_subscription<Rivals>(
-    "/dynamicObstacle", 1, std::bind(&RivalLayer::rival_cb, this, _1));
-}
-
-void RivalLayer::deactivate()
-{
-  auto node = node_.lock();
-  rival_sub_.reset();
-  rivals_.reset();
-}
-
-void RivalLayer::rival_cb(Rivals::SharedPtr msg)
-{
-  if (msg->point.size() != 0 || debounce_++ > 10) {
-    rivals_ = msg;
-    debounce_ = 0;
-  }
-}
-
-void RivalLayer::updateBounds(
-  double, double, double, double * min_x, double * min_y, double * max_x, double * max_y)
-{
-  touch(1.50, 1.0, min_x, min_y, max_x, max_y);
-  touch(-1.50, -1.0, min_x, min_y, max_x, max_y);
-}
-
-void RivalLayer::updateCosts(nav2_costmap_2d::Costmap2D & grid, int, int, int, int)
-{
-  geometry_msgs::msg::TransformStamped tf_msg;
-  if(!rivals_) {
-    return;
-  }
-
-  try {
-    tf_msg = tf_->lookupTransform(
-      layered_costmap_->getGlobalFrameID(), rivals_->header.frame_id, rivals_->header.stamp);
-  } catch (const tf2::TransformException & e) {
-    RCLCPP_WARN_THROTTLE(logger_, *clock_, 1000, "Failed to transform rival message to costmap");
-    return;
-  }
-
-  for (const auto & rival : rivals_->point) {
-    geometry_msgs::msg::Point point;
-    tf2::doTransform(rival, point, tf_msg);
-    placeRival(grid, point.x, point.y);
-  }
-}
-
-void RivalLayer::placeRival(nav2_costmap_2d::Costmap2D & grid, const double x, const double y)
-{
-  unsigned int mx, my;
-  if (!grid.worldToMap(x, y, mx, my)) {
-    return;  // Center is outside the map bounds
-  }
-
-  double res = grid.getResolution();
-  int cell_radius = static_cast<int>(rival_size_ / res);
-
-  int min_i = std::max(0, static_cast<int>(mx) - cell_radius);
-  int max_i =
-    std::min(static_cast<int>(grid.getSizeInCellsX()) - 1, static_cast<int>(mx) + cell_radius);
-  int min_j = std::max(0, static_cast<int>(my) - cell_radius);
-  int max_j =
-    std::min(static_cast<int>(grid.getSizeInCellsY()) - 1, static_cast<int>(my) + cell_radius);
-
-  for (int i = min_i; i <= max_i; ++i) {
-    for (int j = min_j; j <= max_j; ++j) {
-      double di = static_cast<double>(i) - static_cast<double>(mx);
-      double dj = static_cast<double>(j) - static_cast<double>(my);
-      double distance_sq = di * di + dj * dj;
-      if (distance_sq <= static_cast<double>(cell_radius * cell_radius)) {
-        grid.setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE);
-      }
-    }
-  }
-}
-
-}  // namespace toid
-
-#include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(toid::RivalLayer, nav2_costmap_2d::Layer);
--- a/toid_costmaps/toid_costmaps.xml
+++ b/toid_costmaps/toid_costmaps.xml
@@ -1,10 +0,0 @@
-<class_libraries>
-    <library path="toid_costmaps">
-        <class type="toid::GameElementLayer" base_class_type="nav2_costmap_2d::Layer">
-            <description></description>
-        </class>
-        <class type="toid::RivalLayer" base_class_type="nav2_costmap_2d::Layer">
-            <description></description>
-        </class>
-    </library>
-</class_libraries>
--- a/toid_frontend/.editorconfig
+++ b/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
--- a/toid_frontend/.gitattributes
+++ b/toid_frontend/.gitattributes
@@ -0,0 +1 @@
+* text=auto eol=lf
--- a/toid_frontend/.gitignore
+++ b/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
--- a/toid_frontend/.oxlintrc.json
+++ b/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"
+  }
+}
--- a/toid_frontend/.prettierrc.json
+++ b/toid_frontend/.prettierrc.json
@@ -0,0 +1,6 @@
+{
+  "$schema": "https://json.schemastore.org/prettierrc",
+  "semi": false,
+  "singleQuote": true,
+  "printWidth": 100
+}
--- a/toid_frontend/.vscode/extensions.json
+++ b/toid_frontend/.vscode/extensions.json
@@ -0,0 +1,9 @@
+{
+  "recommendations": [
+    "Vue.volar",
+    "dbaeumer.vscode-eslint",
+    "EditorConfig.EditorConfig",
+    "oxc.oxc-vscode",
+    "esbenp.prettier-vscode"
+  ]
+}
--- a/toid_frontend/COLCON_IGNORE
+++ b/toid_frontend/COLCON_IGNORE
--- a/toid_frontend/README.md
+++ b/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
+```
--- a/toid_frontend/env.d.ts
+++ b/toid_frontend/env.d.ts
@@ -0,0 +1 @@
+/// <reference types="vite/client" />
--- a/toid_frontend/eslint.config.ts
+++ b/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,
+)
--- a/toid_frontend/index.html
+++ b/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>
--- a/toid_frontend/package-lock.json
+++ b/toid_frontend/package-lock.json
--- a/toid_frontend/package.json
+++ b/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"
+  }
+}
--- a/toid_frontend/public/favicon.ico
+++ b/toid_frontend/public/favicon.ico
--- a/toid_frontend/src/App.vue
+++ b/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>
--- a/toid_frontend/src/assets/images/table.png
+++ b/toid_frontend/src/assets/images/table.png
--- a/toid_frontend/src/assets/images/table.svg
+++ b/toid_frontend/src/assets/images/table.svg
--- a/toid_frontend/src/assets/scss/global.scss
+++ b/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;
+}
--- a/toid_frontend/src/components/PageBanner.vue
+++ b/toid_frontend/src/components/PageBanner.vue
@@ -0,0 +1,7 @@
+<script lang="ts"></script>
+
+<template>
+  <div>Banner</div>
+</template>
+
+<style scoped></style>
--- a/toid_frontend/src/components/RobotControl.vue
+++ b/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>
--- a/toid_frontend/src/components/RobotViewport.vue
+++ b/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>
--- a/toid_frontend/src/main.ts
+++ b/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')
--- a/toid_frontend/src/router/index.ts
+++ b/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
--- a/toid_frontend/src/stores/connection-status.ts
+++ b/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 }
+})
--- a/toid_frontend/src/stores/counter.ts
+++ b/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 }
+})
--- a/toid_frontend/src/ts/robot_bridge.ts
+++ b/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 }
--- a/toid_frontend/src/ts/ros_client.ts
+++ b/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 }
--- a/toid_frontend/src/utils/smoothControlLaw.ts
+++ b/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 }
--- a/toid_frontend/tsconfig.app.json
+++ b/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"
+  }
+}
--- a/toid_frontend/tsconfig.json
+++ b/toid_frontend/tsconfig.json
@@ -0,0 +1,11 @@
+{
+  "files": [],
+  "references": [
+    {
+      "path": "./tsconfig.node.json"
+    },
+    {
+      "path": "./tsconfig.app.json"
+    }
+  ]
+}
--- a/toid_frontend/tsconfig.node.json
+++ b/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"
+  }
+}
--- a/toid_frontend/vite.config.ts
+++ b/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))
+    },
+  },
+})
--- a/toid_interaction/setup.cfg
+++ b/toid_interaction/setup.cfg
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/toid_interaction
-[install]
-install_scripts=$base/lib/toid_interaction
--- a/toid_interaction/test/test_copyright.py
+++ b/toid_interaction/test/test_copyright.py
@@ -1,25 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
--- a/toid_interaction/test/test_flake8.py
+++ b/toid_interaction/test/test_flake8.py
@@ -1,25 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
-    rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
--- a/toid_interaction/test/test_pep257.py
+++ b/toid_interaction/test/test_pep257.py
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'
--- a/toid_interaction/toid_interaction/pycache/init.cpython-312.pyc
+++ b/toid_interaction/toid_interaction/pycache/init.cpython-312.pyc
--- a/toid_interaction/toid_interaction/pycache/interaction_node.cpython-312.pyc
+++ b/toid_interaction/toid_interaction/pycache/interaction_node.cpython-312.pyc
--- a/toid_interaction/toid_interaction/interaction_node.py
+++ b/toid_interaction/toid_interaction/interaction_node.py
@@ -1,97 +0,0 @@
-import rclpy
-from rclpy.node import Node
-from std_srvs.srv import Empty
-
-from serial.tools import list_ports
-
-from toid_interaction.mechanism.sekvenca_2026 import okreni, okreni_niz
-from toid_interaction.mechanism.zidovi_load import ZidoviAction
-from toid_interaction.mechanism.zupcanik import ZupcanikAction
-from toid_msgs.srv import SendString
-
-class InteracitionNode(Node):
-    step: int = 0
-    def __init__(self):
-        super().__init__('ToidInteractionNode')
-
-        self.find_sigma()
-
-        self.srv = self.create_service(
-            Empty,
-            '/sequence1',
-            self.sequence1_cb
-        )
-
-        self.srv = self.create_service(
-            SendString,
-            '/sequence2',
-            self.sequence2_cb
-        )
-
-        self.srv = self.create_service(
-            Empty,
-            '/sequence3',
-            self.sequence3_cb
-        )
-
-
-
-        self.get_logger().info("Service 'sequence1' ready.")
-
-    def find_sigma(self):
-        for port_info in list_ports.comports():
-            if port_info.vid == 0x18a6 and port_info.pid == 0x55d3:
-                break
-
-        print(port_info.device)
-        self.st_motor_device_name = port_info.device
-
-    def sequence1_cb(self, request, response):
-        if(self.step != 0):
-            return Empty.Response()
-        okreni(5)
-        zupcanik = ZupcanikAction(self.st_motor_device_name)
-        zupcanik.zupcanik(1, -1010, 25)
-        self.step = 1
-        return response
-
-    def sequence2_cb(self, request : SendString.Request, response : SendString.Response):
-        if(self.step != 1):
-            return Empty.Response()
-        zidovi = ZidoviAction(self.st_motor_device_name)
-        zidovi.beli_zid(1)
-        zidovi.plavi_zid(1)
-
-        okreni_niz(request.text)
-
-        zidovi.plavi_zid(0, TargetPos=150)
-        zidovi.beli_zid(0, TargetPos=450)
-        self.step = 2
-        return response
-
-    def sequence3_cb(self, request, response):
-        if(self.step != 2):
-            return Empty.Response()
-
-        zupcanik = ZupcanikAction(self.st_motor_device_name)
-        zidovi = ZidoviAction(self.st_motor_device_name) 
-
-        zupcanik.zupcanik(1, 1010, 25)
-        zidovi.plavi_zid(0, TargetPos=150)
-        zidovi.beli_zid(0, TargetPos=150)
-        okreni(5)
-        self.step = 0
-        return response
-
-
-def main(args=None):
-    rclpy.init(args=args)
-
-    node = InteracitionNode()
-    rclpy.spin(node)
-
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/init.py
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/init.py
@@ -1,8 +0,0 @@
-#!/usr/bin/env python
-
-from .port_handler import *
-from .protocol_packet_handler import *
-from .group_sync_write import *
-from .group_sync_read import *
-from .sts import *
-from .scscl import *
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/init.cpython-312.pyc
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/init.cpython-312.pyc
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/group_sync_read.cpython-312.pyc
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/group_sync_read.cpython-312.pyc
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/group_sync_write.cpython-312.pyc
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/group_sync_write.cpython-312.pyc
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/port_handler.cpython-312.pyc
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/port_handler.cpython-312.pyc
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/protocol_packet_handler.cpython-312.pyc
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/protocol_packet_handler.cpython-312.pyc
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/scscl.cpython-312.pyc
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/scscl.cpython-312.pyc
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/sts.cpython-312.pyc
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/sts.cpython-312.pyc
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/stservo_def.cpython-312.pyc
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/pycache/stservo_def.cpython-312.pyc
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py
@@ -1,151 +0,0 @@
-#!/usr/bin/env python
-
-from .stservo_def import *
-
-class GroupSyncRead:
-    def __init__(self, ph, start_address, data_length):
-        self.ph = ph
-        self.start_address = start_address
-        self.data_length = data_length
-
-        self.last_result = False
-        self.is_param_changed = False
-        self.param = []
-        self.data_dict = {}
-
-        self.clearParam()
-
-    def makeParam(self):
-        if not self.data_dict:  # len(self.data_dict.keys()) == 0:
-            return
-
-        self.param = []
-
-        for scs_id in self.data_dict:
-            self.param.append(scs_id)
-
-    def addParam(self, sts_id):
-        if sts_id in self.data_dict:  # sts_id already exist
-            return False
-
-        self.data_dict[sts_id] = []  # [0] * self.data_length
-
-        self.is_param_changed = True
-        return True
-
-    def removeParam(self, sts_id):
-        if sts_id not in self.data_dict:  # NOT exist
-            return
-
-        del self.data_dict[sts_id]
-
-        self.is_param_changed = True
-
-    def clearParam(self):
-        self.data_dict.clear()
-
-    def txPacket(self):
-        if len(self.data_dict.keys()) == 0:
-
-            return COMM_NOT_AVAILABLE
-
-        if self.is_param_changed is True or not self.param:
-            self.makeParam()
-
-        return self.ph.syncReadTx(self.start_address, self.data_length, self.param, len(self.data_dict.keys()))
-
-    def rxPacket(self):
-        self.last_result = True
-
-        result = COMM_RX_FAIL
-
-        if len(self.data_dict.keys()) == 0:
-            return COMM_NOT_AVAILABLE
-
-        result, rxpacket = self.ph.syncReadRx(self.data_length, len(self.data_dict.keys()))
-        # print(rxpacket)
-        if len(rxpacket) >= (self.data_length+6):
-            for sts_id in self.data_dict:
-                self.data_dict[sts_id], result = self.readRx(rxpacket, sts_id, self.data_length)
-                if result != COMM_SUCCESS:
-                    self.last_result = False
-                # print(sts_id)
-        else:
-            self.last_result = False
-        # print(self.last_result)
-        return result
-
-    def txRxPacket(self):
-        result = self.txPacket()
-        if result != COMM_SUCCESS:
-            return result
-
-        return self.rxPacket()
-
-    def readRx(self, rxpacket, sts_id, data_length):
-        # print(sts_id)
-        # print(rxpacket)
-        data = []
-        rx_length = len(rxpacket)
-        # print(rx_length)
-        rx_index = 0;
-        while (rx_index+6+data_length) <= rx_length:
-            headpacket = [0x00, 0x00, 0x00]
-            while rx_index < rx_length:
-                headpacket[2] = headpacket[1];
-                headpacket[1] = headpacket[0];
-                headpacket[0] = rxpacket[rx_index];
-                rx_index += 1
-                if (headpacket[2] == 0xFF) and (headpacket[1] == 0xFF) and headpacket[0] == sts_id:
-                    # print(rx_index)
-                    break
-            # print(rx_index+3+data_length)
-            if (rx_index+3+data_length) > rx_length:
-                break;
-            if rxpacket[rx_index] != (data_length+2):
-                rx_index += 1
-                # print(rx_index)
-                continue
-            rx_index += 1
-            Error = rxpacket[rx_index]
-            rx_index += 1
-            calSum = sts_id + (data_length+2) + Error
-            data = [Error]
-            data.extend(rxpacket[rx_index : rx_index+data_length])
-            for i in range(0, data_length):
-                calSum += rxpacket[rx_index]
-                rx_index += 1
-            calSum = ~calSum & 0xFF
-            # print(calSum)
-            if calSum != rxpacket[rx_index]:
-                return None, COMM_RX_CORRUPT
-            return data, COMM_SUCCESS 
-        # print(rx_index)
-        return None, COMM_RX_CORRUPT
-
-    def isAvailable(self, sts_id, address, data_length):
-        #if self.last_result is False or sts_id not in self.data_dict:
-        if sts_id not in self.data_dict:
-            return False, 0
-
-        if (address < self.start_address) or (self.start_address + self.data_length - data_length < address):
-            return False, 0
-        if not self.data_dict[sts_id]:
-            return False, 0
-        if len(self.data_dict[sts_id])<(data_length+1):
-            return False, 0
-        return True, self.data_dict[sts_id][0]
-
-    def getData(self, sts_id, address, data_length):
-        if data_length == 1:
-            return self.data_dict[sts_id][address-self.start_address+1]
-        elif data_length == 2:
-            return self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+1],
-                                self.data_dict[sts_id][address-self.start_address+2])
-        elif data_length == 4:
-            return self.ph.scs_makedword(self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+1],
-                                              self.data_dict[sts_id][address-self.start_address+2]),
-                                 self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+3],
-                                              self.data_dict[sts_id][address-self.start_address+4]))
-        else:
-            return 0
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py
@@ -1,73 +0,0 @@
-#!/usr/bin/env python
-
-from .stservo_def import *
-
-class GroupSyncWrite:
-    def __init__(self, ph, start_address, data_length):
-        self.ph = ph
-        self.start_address = start_address
-        self.data_length = data_length
-
-        self.is_param_changed = False
-        self.param = []
-        self.data_dict = {}
-
-        self.clearParam()
-
-    def makeParam(self):
-        if not self.data_dict:
-            return
-
-        self.param = []
-
-        for sts_id in self.data_dict:
-            if not self.data_dict[sts_id]:
-                return
-
-            self.param.append(sts_id)
-            self.param.extend(self.data_dict[sts_id])
-
-    def addParam(self, sts_id, data):
-        if sts_id in self.data_dict:  # sts_id already exist
-            return False
-
-        if len(data) > self.data_length:  # input data is longer than set
-            return False
-
-        self.data_dict[sts_id] = data
-
-        self.is_param_changed = True
-        return True
-
-    def removeParam(self, sts_id):
-        if sts_id not in self.data_dict:  # NOT exist
-            return
-
-        del self.data_dict[sts_id]
-
-        self.is_param_changed = True
-
-    def changeParam(self, sts_id, data):
-        if sts_id not in self.data_dict:  # NOT exist
-            return False
-
-        if len(data) > self.data_length:  # input data is longer than set
-            return False
-
-        self.data_dict[sts_id] = data
-
-        self.is_param_changed = True
-        return True
-
-    def clearParam(self):
-        self.data_dict.clear()
-
-    def txPacket(self):
-        if len(self.data_dict.keys()) == 0:
-            return COMM_NOT_AVAILABLE
-
-        if self.is_param_changed is True or not self.param:
-            self.makeParam()
-
-        return self.ph.syncWriteTxOnly(self.start_address, self.data_length, self.param,
-                                       len(self.data_dict.keys()) * (1 + self.data_length))
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py
@@ -1,115 +0,0 @@
-#!/usr/bin/env python
-
-import time
-import serial
-import sys
-import platform
-
-DEFAULT_BAUDRATE = 1000000
-LATENCY_TIMER = 50 
-
-class PortHandler(object):
-    def __init__(self, port_name):
-        self.is_open = False
-        self.baudrate = DEFAULT_BAUDRATE
-        self.packet_start_time = 0.0
-        self.packet_timeout = 0.0
-        self.tx_time_per_byte = 0.0
-
-        self.is_using = False
-        self.port_name = port_name
-        self.ser = None
-
-    def openPort(self):
-        return self.setBaudRate(self.baudrate)
-
-    def closePort(self):
-        self.ser.close()
-        self.is_open = False
-
-    def clearPort(self):
-        self.ser.flush()
-
-    def setPortName(self, port_name):
-        self.port_name = port_name
-
-    def getPortName(self):
-        return self.port_name
-
-    def setBaudRate(self, baudrate):
-        baud = self.getCFlagBaud(baudrate)
-
-        if baud <= 0:
-            # self.setupPort(38400)
-            # self.baudrate = baudrate
-            return False  # TODO: setCustomBaudrate(baudrate)
-        else:
-            self.baudrate = baudrate
-            return self.setupPort(baud)
-
-    def getBaudRate(self):
-        return self.baudrate
-
-    def getBytesAvailable(self):
-        return self.ser.in_waiting
-
-    def readPort(self, length):
-        if (sys.version_info > (3, 0)):
-            return self.ser.read(length)
-        else:
-            return [ord(ch) for ch in self.ser.read(length)]
-
-    def writePort(self, packet):
-        return self.ser.write(packet)
-
-    def setPacketTimeout(self, packet_length):
-        self.packet_start_time = self.getCurrentTime()
-        self.packet_timeout = (self.tx_time_per_byte * packet_length) + (self.tx_time_per_byte * 3.0) + LATENCY_TIMER
-
-    def setPacketTimeoutMillis(self, msec):
-        self.packet_start_time = self.getCurrentTime()
-        self.packet_timeout = msec
-
-    def isPacketTimeout(self):
-        if self.getTimeSinceStart() > self.packet_timeout:
-            self.packet_timeout = 0
-            return True
-
-        return False
-
-    def getCurrentTime(self):
-        return round(time.time() * 1000000000) / 1000000.0
-
-    def getTimeSinceStart(self):
-        time_since = self.getCurrentTime() - self.packet_start_time
-        if time_since < 0.0:
-            self.packet_start_time = self.getCurrentTime()
-
-        return time_since
-
-    def setupPort(self, cflag_baud):
-        if self.is_open:
-            self.closePort()
-
-        self.ser = serial.Serial(
-            port=self.port_name,
-            baudrate=self.baudrate,
-            # parity = serial.PARITY_ODD,
-            # stopbits = serial.STOPBITS_TWO,
-            bytesize=serial.EIGHTBITS,
-            timeout=0
-        )
-
-        self.is_open = True
-
-        self.ser.reset_input_buffer()
-
-        self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0
-
-        return True
-
-    def getCFlagBaud(self, baudrate):
-        if baudrate in [4800, 9600, 14400, 19200, 38400, 57600, 115200, 128000, 250000, 500000, 1000000]:
-            return baudrate
-        else:
-            return -1          
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py
@@ -1,530 +0,0 @@
-#!/usr/bin/env python
-
-from .stservo_def import *
-
-TXPACKET_MAX_LEN = 250
-RXPACKET_MAX_LEN = 250
-
-# for Protocol Packet
-PKT_HEADER0 = 0
-PKT_HEADER1 = 1
-PKT_ID = 2
-PKT_LENGTH = 3
-PKT_INSTRUCTION = 4
-PKT_ERROR = 4
-PKT_PARAMETER0 = 5
-
-# Protocol Error bit
-ERRBIT_VOLTAGE = 1
-ERRBIT_ANGLE = 2
-ERRBIT_OVERHEAT = 4
-ERRBIT_OVERELE = 8
-ERRBIT_OVERLOAD = 32
-
-
-class protocol_packet_handler(object):
-    def __init__(self, portHandler, protocol_end):
-        #self.sts_setend(protocol_end)# STServo bit end(STS/SMS=0, SCS=1)
-        self.portHandler = portHandler
-        self.sts_end = protocol_end
-
-    def sts_getend(self):
-        return self.sts_end
-
-    def sts_setend(self, e):
-        self.sts_end = e
-
-    def sts_tohost(self, a, b):
-        if (a & (1<<b)):
-            return -(a & ~(1<<b))
-        else:
-            return a
-
-    def sts_toscs(self, a, b):
-        if (a<0):
-            return (-a | (1<<b))
-        else:
-            return a
-
-    def sts_makeword(self, a, b):
-        if self.sts_end==0:
-            return (a & 0xFF) | ((b & 0xFF) << 8)
-        else:
-            return (b & 0xFF) | ((a & 0xFF) << 8)
-
-    def sts_makedword(self, a, b):
-        return (a & 0xFFFF) | (b & 0xFFFF) << 16
-
-    def sts_loword(self, l):
-        return l & 0xFFFF
-
-    def sts_hiword(self, h):
-        return (h >> 16) & 0xFFFF
-
-    def sts_lobyte(self, w):
-        if self.sts_end==0:
-            return w & 0xFF
-        else:
-            return (w >> 8) & 0xFF
-
-    def sts_hibyte(self, w):
-        if self.sts_end==0:
-            return (w >> 8) & 0xFF
-        else:
-            return w & 0xFF
-        
-    def getProtocolVersion(self):
-        return 1.0
-
-    def getTxRxResult(self, result):
-        if result == COMM_SUCCESS:
-            return "[TxRxResult] Communication success!"
-        elif result == COMM_PORT_BUSY:
-            return "[TxRxResult] Port is in use!"
-        elif result == COMM_TX_FAIL:
-            return "[TxRxResult] Failed transmit instruction packet!"
-        elif result == COMM_RX_FAIL:
-            return "[TxRxResult] Failed get status packet from device!"
-        elif result == COMM_TX_ERROR:
-            return "[TxRxResult] Incorrect instruction packet!"
-        elif result == COMM_RX_WAITING:
-            return "[TxRxResult] Now receiving status packet!"
-        elif result == COMM_RX_TIMEOUT:
-            return "[TxRxResult] There is no status packet!"
-        elif result == COMM_RX_CORRUPT:
-            return "[TxRxResult] Incorrect status packet!"
-        elif result == COMM_NOT_AVAILABLE:
-            return "[TxRxResult] Protocol does not support this function!"
-        else:
-            return ""
-
-    def getRxPacketError(self, error):
-        if error & ERRBIT_VOLTAGE:
-            return "[ServoStatus] Input voltage error!"
-
-        if error & ERRBIT_ANGLE:
-            return "[ServoStatus] Angle sen error!"
-
-        if error & ERRBIT_OVERHEAT:
-            return "[ServoStatus] Overheat error!"
-
-        if error & ERRBIT_OVERELE:
-            return "[ServoStatus] OverEle error!"
-        
-        if error & ERRBIT_OVERLOAD:
-            return "[ServoStatus] Overload error!"
-
-        return ""
-
-    def txPacket(self, txpacket):
-        checksum = 0
-        total_packet_length = txpacket[PKT_LENGTH] + 4  # 4: HEADER0 HEADER1 ID LENGTH
-
-        if self.portHandler.is_using:
-            return COMM_PORT_BUSY
-        self.portHandler.is_using = True
-
-        # check max packet length
-        if total_packet_length > TXPACKET_MAX_LEN:
-            self.portHandler.is_using = False
-            return COMM_TX_ERROR
-
-        # make packet header
-        txpacket[PKT_HEADER0] = 0xFF
-        txpacket[PKT_HEADER1] = 0xFF
-
-        # add a checksum to the packet
-        for idx in range(2, total_packet_length - 1):  # except header, checksum
-            checksum += txpacket[idx]
-
-        txpacket[total_packet_length - 1] = ~checksum & 0xFF
-
-        #print "[TxPacket] %r" % txpacket
-
-        # tx packet
-        self.portHandler.clearPort()
-        written_packet_length = self.portHandler.writePort(txpacket)
-        if total_packet_length != written_packet_length:
-            self.portHandler.is_using = False
-            return COMM_TX_FAIL
-
-        return COMM_SUCCESS
-
-    def rxPacket(self):
-        rxpacket = []
-
-        result = COMM_TX_FAIL
-        checksum = 0
-        rx_length = 0
-        wait_length = 6  # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM)
-
-        while True:
-            rxpacket.extend(self.portHandler.readPort(wait_length - rx_length))
-            rx_length = len(rxpacket)
-            if rx_length >= wait_length:
-                # find packet header
-                for idx in range(0, (rx_length - 1)):
-                    if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF):
-                        break
-
-                if idx == 0:  # found at the beginning of the packet
-                    if (rxpacket[PKT_ID] > 0xFD) or (rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) or (
-                            rxpacket[PKT_ERROR] > 0x7F):
-                        # unavailable ID or unavailable Length or unavailable Error
-                        # remove the first byte in the packet
-                        del rxpacket[0]
-                        rx_length -= 1
-                        continue
-
-                    # re-calculate the exact length of the rx packet
-                    if wait_length != (rxpacket[PKT_LENGTH] + PKT_LENGTH + 1):
-                        wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1
-                        continue
-
-                    if rx_length < wait_length:
-                        # check timeout
-                        if self.portHandler.isPacketTimeout():
-                            if rx_length == 0:
-                                result = COMM_RX_TIMEOUT
-                            else:
-                                result = COMM_RX_CORRUPT
-                            break
-                        else:
-                            continue
-
-                    # calculate checksum
-                    for i in range(2, wait_length - 1):  # except header, checksum
-                        checksum += rxpacket[i]
-                    checksum = ~checksum & 0xFF
-
-                    # verify checksum
-                    if rxpacket[wait_length - 1] == checksum:
-                        result = COMM_SUCCESS
-                    else:
-                        result = COMM_RX_CORRUPT
-                    break
-
-                else:
-                    # remove unnecessary packets
-                    del rxpacket[0: idx]
-                    rx_length -= idx
-
-            else:
-                # check timeout
-                if self.portHandler.isPacketTimeout():
-                    if rx_length == 0:
-                        result = COMM_RX_TIMEOUT
-                    else:
-                        result = COMM_RX_CORRUPT
-                    break
-
-        self.portHandler.is_using = False
-        return rxpacket, result
-
-    def txRxPacket(self, txpacket):
-        rxpacket = None
-        error = 0
-
-        # tx packet
-        result = self.txPacket(txpacket)
-        if result != COMM_SUCCESS:
-            return rxpacket, result, error
-
-        # (ID == Broadcast ID) == no need to wait for status packet or not available
-        if (txpacket[PKT_ID] == BROADCAST_ID):
-            self.portHandler.is_using = False
-            return rxpacket, result, error
-
-        # set packet timeout
-        if txpacket[PKT_INSTRUCTION] == INST_READ:
-            self.portHandler.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6)
-        else:
-            self.portHandler.setPacketTimeout(6)  # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM
-
-        # rx packet
-        while True:
-            rxpacket, result = self.rxPacket()
-            if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]:
-                break
-
-        if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]:
-            error = rxpacket[PKT_ERROR]
-
-        return rxpacket, result, error
-
-    def ping(self, sts_id):
-        model_number = 0
-        error = 0
-
-        txpacket = [0] * 6
-
-        if sts_id >= BROADCAST_ID:
-            return model_number, COMM_NOT_AVAILABLE, error
-
-        txpacket[PKT_ID] = sts_id
-        txpacket[PKT_LENGTH] = 2
-        txpacket[PKT_INSTRUCTION] = INST_PING
-
-        rxpacket, result, error = self.txRxPacket(txpacket)
-
-        if result == COMM_SUCCESS:
-            data_read, result, error = self.readTxRx(sts_id, 3, 2)  # Address 3 : Model Number
-            if result == COMM_SUCCESS:
-                model_number = self.sts_makeword(data_read[0], data_read[1])
-
-        return model_number, result, error
-
-    def action(self, sts_id):
-        txpacket = [0] * 6
-
-        txpacket[PKT_ID] = sts_id
-        txpacket[PKT_LENGTH] = 2
-        txpacket[PKT_INSTRUCTION] = INST_ACTION
-
-        _, result, _ = self.txRxPacket(txpacket)
-
-        return result
-
-    def readTx(self, sts_id, address, length):
-
-        txpacket = [0] * 8
-
-        if sts_id >= BROADCAST_ID:
-            return COMM_NOT_AVAILABLE
-
-        txpacket[PKT_ID] = sts_id
-        txpacket[PKT_LENGTH] = 4
-        txpacket[PKT_INSTRUCTION] = INST_READ
-        txpacket[PKT_PARAMETER0 + 0] = address
-        txpacket[PKT_PARAMETER0 + 1] = length
-
-        result = self.txPacket(txpacket)
-
-        # set packet timeout
-        if result == COMM_SUCCESS:
-            self.portHandler.setPacketTimeout(length + 6)
-
-        return result
-
-    def readRx(self, sts_id, length):
-        result = COMM_TX_FAIL
-        error = 0
-
-        rxpacket = None
-        data = []
-
-        while True:
-            rxpacket, result = self.rxPacket()
-
-            if result != COMM_SUCCESS or rxpacket[PKT_ID] == sts_id:
-                break
-
-        if result == COMM_SUCCESS and rxpacket[PKT_ID] == sts_id:
-            error = rxpacket[PKT_ERROR]
-
-            data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0+length])
-
-        return data, result, error
-
-    def readTxRx(self, sts_id, address, length):
-        txpacket = [0] * 8
-        data = []
-
-        if sts_id >= BROADCAST_ID:
-            return data, COMM_NOT_AVAILABLE, 0
-
-        txpacket[PKT_ID] = sts_id
-        txpacket[PKT_LENGTH] = 4
-        txpacket[PKT_INSTRUCTION] = INST_READ
-        txpacket[PKT_PARAMETER0 + 0] = address
-        txpacket[PKT_PARAMETER0 + 1] = length
-
-        rxpacket, result, error = self.txRxPacket(txpacket)
-        if result == COMM_SUCCESS:
-            error = rxpacket[PKT_ERROR]
-
-            data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0+length])
-
-        return data, result, error
-
-    def read1ByteTx(self, sts_id, address):
-        return self.readTx(sts_id, address, 1)
-
-    def read1ByteRx(self, sts_id):
-        data, result, error = self.readRx(sts_id, 1)
-        data_read = data[0] if (result == COMM_SUCCESS) else 0
-        return data_read, result, error
-
-    def read1ByteTxRx(self, sts_id, address):
-        data, result, error = self.readTxRx(sts_id, address, 1)
-        data_read = data[0] if (result == COMM_SUCCESS) else 0
-        return data_read, result, error
-
-    def read2ByteTx(self, sts_id, address):
-        return self.readTx(sts_id, address, 2)
-
-    def read2ByteRx(self, sts_id):
-        data, result, error = self.readRx(sts_id, 2)
-        data_read = self.sts_makeword(data[0], data[1]) if (result == COMM_SUCCESS) else 0
-        return data_read, result, error
-
-    def read2ByteTxRx(self, sts_id, address):
-        data, result, error = self.readTxRx(sts_id, address, 2)
-        data_read = self.sts_makeword(data[0], data[1]) if (result == COMM_SUCCESS) else 0
-        return data_read, result, error
-
-    def read4ByteTx(self, sts_id, address):
-        return self.readTx(sts_id, address, 4)
-
-    def read4ByteRx(self, sts_id):
-        data, result, error = self.readRx(sts_id, 4)
-        data_read = self.sts_makedword(self.sts_makeword(data[0], data[1]),
-                                  self.sts_makeword(data[2], data[3])) if (result == COMM_SUCCESS) else 0
-        return data_read, result, error
-
-    def read4ByteTxRx(self, sts_id, address):
-        data, result, error = self.readTxRx(sts_id, address, 4)
-        data_read = self.sts_makedword(self.sts_makeword(data[0], data[1]),
-                                  self.sts_makeword(data[2], data[3])) if (result == COMM_SUCCESS) else 0
-        return data_read, result, error
-
-    def writeTxOnly(self, sts_id, address, length, data):
-        txpacket = [0] * (length + 7)
-
-        txpacket[PKT_ID] = sts_id
-        txpacket[PKT_LENGTH] = length + 3
-        txpacket[PKT_INSTRUCTION] = INST_WRITE
-        txpacket[PKT_PARAMETER0] = address
-
-        txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
-
-        result = self.txPacket(txpacket)
-        self.portHandler.is_using = False
-
-        return result
-
-    def writeTxRx(self, sts_id, address, length, data):
-        txpacket = [0] * (length + 7)
-
-        txpacket[PKT_ID] = sts_id
-        txpacket[PKT_LENGTH] = length + 3
-        txpacket[PKT_INSTRUCTION] = INST_WRITE
-        txpacket[PKT_PARAMETER0] = address
-
-        txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
-        rxpacket, result, error = self.txRxPacket(txpacket)
-
-        return result, error
-
-    def write1ByteTxOnly(self, sts_id, address, data):
-        data_write = [data]
-        return self.writeTxOnly(sts_id, address, 1, data_write)
-
-    def write1ByteTxRx(self, sts_id, address, data):
-        data_write = [data]
-        return self.writeTxRx(sts_id, address, 1, data_write)
-
-    def write2ByteTxOnly(self, sts_id, address, data):
-        data_write = [self.sts_lobyte(data), self.sts_hibyte(data)]
-        return self.writeTxOnly(sts_id, address, 2, data_write)
-
-    def write2ByteTxRx(self, sts_id, address, data):
-        data_write = [self.sts_lobyte(data), self.sts_hibyte(data)]
-        return self.writeTxRx(sts_id, address, 2, data_write)
-
-    def write4ByteTxOnly(self, sts_id, address, data):
-        data_write = [self.sts_lobyte(self.sts_loword(data)),
-                      self.sts_hibyte(self.sts_loword(data)),
-                      self.sts_lobyte(self.sts_hiword(data)),
-                      self.sts_hibyte(self.sts_hiword(data))]
-        return self.writeTxOnly(sts_id, address, 4, data_write)
-
-    def write4ByteTxRx(self, sts_id, address, data):
-        data_write = [self.sts_lobyte(self.sts_loword(data)),
-                      self.sts_hibyte(self.sts_loword(data)),
-                      self.sts_lobyte(self.sts_hiword(data)),
-                      self.sts_hibyte(self.sts_hiword(data))]
-        return self.writeTxRx(sts_id, address, 4, data_write)
-
-    def regWriteTxOnly(self, sts_id, address, length, data):
-        txpacket = [0] * (length + 7)
-
-        txpacket[PKT_ID] = sts_id
-        txpacket[PKT_LENGTH] = length + 3
-        txpacket[PKT_INSTRUCTION] = INST_REG_WRITE
-        txpacket[PKT_PARAMETER0] = address
-
-        txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
-
-        result = self.txPacket(txpacket)
-        self.portHandler.is_using = False
-
-        return result
-
-    def regWriteTxRx(self, sts_id, address, length, data):
-        txpacket = [0] * (length + 7)
-
-        txpacket[PKT_ID] = sts_id
-        txpacket[PKT_LENGTH] = length + 3
-        txpacket[PKT_INSTRUCTION] = INST_REG_WRITE
-        txpacket[PKT_PARAMETER0] = address
-
-        txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
-
-        _, result, error = self.txRxPacket(txpacket)
-
-        return result, error
-
-    def syncReadTx(self, start_address, data_length, param, param_length):
-        txpacket = [0] * (param_length + 8)
-        # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN CHKSUM
-
-        txpacket[PKT_ID] = BROADCAST_ID
-        txpacket[PKT_LENGTH] = param_length + 4  # 7: INST START_ADDR DATA_LEN CHKSUM
-        txpacket[PKT_INSTRUCTION] = INST_SYNC_READ
-        txpacket[PKT_PARAMETER0 + 0] = start_address
-        txpacket[PKT_PARAMETER0 + 1] = data_length
-
-        txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length]
-
-        # print(txpacket)
-        result = self.txPacket(txpacket)
-        return result
-
-    def syncReadRx(self, data_length, param_length):
-        wait_length = (6 + data_length) * param_length
-        self.portHandler.setPacketTimeout(wait_length)
-        rxpacket = []
-        rx_length = 0
-        while True:
-            rxpacket.extend(self.portHandler.readPort(wait_length - rx_length))
-            rx_length = len(rxpacket)
-            if rx_length >= wait_length:
-                result = COMM_SUCCESS
-                break
-            else:
-                # check timeout
-                if self.portHandler.isPacketTimeout():
-                    if rx_length == 0:
-                        result = COMM_RX_TIMEOUT
-                    else:
-                        result = COMM_RX_CORRUPT
-                    break
-        self.portHandler.is_using = False
-        return result, rxpacket
-
-    def syncWriteTxOnly(self, start_address, data_length, param, param_length):
-        txpacket = [0] * (param_length + 8)
-        # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM
-
-        txpacket[PKT_ID] = BROADCAST_ID
-        txpacket[PKT_LENGTH] = param_length + 4  # 4: INST START_ADDR DATA_LEN ... CHKSUM
-        txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE
-        txpacket[PKT_PARAMETER0 + 0] = start_address
-        txpacket[PKT_PARAMETER0 + 1] = data_length
-
-        txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length]
-
-        _, result, _ = self.txRxPacket(txpacket)
-
-        return result
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py
@@ -1,104 +0,0 @@
-#!/usr/bin/env python
-
-from .stservo_def import *
-from .protocol_packet_handler import *
-from .group_sync_write import *
-
-#波特率定义
-SCSCL_1M = 0
-SCSCL_0_5M = 1
-SCSCL_250K = 2
-SCSCL_128K = 3
-SCSCL_115200 = 4
-SCSCL_76800 = 5
-SCSCL_57600 = 6
-SCSCL_38400 = 7
-
-#内存表定义
-#-------EPROM(只读)--------
-SCSCL_MODEL_L = 3
-SCSCL_MODEL_H = 4
-
-#-------EPROM(读写)--------
-scs_id = 5
-SCSCL_BAUD_RATE = 6
-SCSCL_MIN_ANGLE_LIMIT_L = 9
-SCSCL_MIN_ANGLE_LIMIT_H = 10
-SCSCL_MAX_ANGLE_LIMIT_L = 11
-SCSCL_MAX_ANGLE_LIMIT_H = 12
-SCSCL_CW_DEAD = 26
-SCSCL_CCW_DEAD = 27
-
-#-------SRAM(读写)--------
-SCSCL_TORQUE_ENABLE = 40
-SCSCL_GOAL_POSITION_L = 42
-SCSCL_GOAL_POSITION_H = 43
-SCSCL_GOAL_TIME_L = 44
-SCSCL_GOAL_TIME_H = 45
-SCSCL_GOAL_SPEED_L = 46
-SCSCL_GOAL_SPEED_H = 47
-SCSCL_LOCK = 48
-
-#-------SRAM(只读)--------
-SCSCL_PRESENT_POSITION_L  = 56
-SCSCL_PRESENT_POSITION_H = 57
-SCSCL_PRESENT_SPEED_L = 58
-SCSCL_PRESENT_SPEED_H = 59
-SCSCL_PRESENT_LOAD_L = 60
-SCSCL_PRESENT_LOAD_H = 61
-SCSCL_PRESENT_VOLTAGE = 62
-SCSCL_PRESENT_TEMPERATURE = 63
-SCSCL_MOVING = 66
-SCSCL_PRESENT_CURRENT_L = 69
-SCSCL_PRESENT_CURRENT_H = 70
-
-class scscl(protocol_packet_handler):
-    def __init__(self, portHandler):
-        protocol_packet_handler.__init__(self, portHandler, 1)
-        self.groupSyncWrite = GroupSyncWrite(self, SCSCL_GOAL_POSITION_L, 6)
-
-    def WritePos(self, scs_id, position, time, speed):
-        txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)]
-        return self.writeTxRx(scs_id, SCSCL_GOAL_POSITION_L, len(txpacket), txpacket)
-
-    def ReadPos(self, scs_id):
-        scs_present_position, scs_comm_result, scs_error = self.read2ByteTxRx(scs_id, SCSCL_PRESENT_POSITION_L)
-        return scs_present_position, scs_comm_result, scs_error
-
-    def ReadSpeed(self, scs_id):
-        scs_present_speed, scs_comm_result, scs_error = self.read2ByteTxRx(scs_id, SCSCL_PRESENT_SPEED_L)
-        return self.scs_tohost(scs_present_speed, 15), scs_comm_result, scs_error
-
-    def ReadPosSpeed(self, scs_id):
-        scs_present_position_speed, scs_comm_result, scs_error = self.read4ByteTxRx(scs_id, SCSCL_PRESENT_POSITION_L)
-        scs_present_position = self.scs_loword(scs_present_position_speed)
-        scs_present_speed = self.scs_hiword(scs_present_position_speed)
-        return scs_present_position, self.scs_tohost(scs_present_speed, 15), scs_comm_result, scs_error
-
-    def ReadMoving(self, scs_id):
-        moving, scs_comm_result, scs_error = self.read1ByteTxRx(scs_id, SCSCL_MOVING)
-        return moving, scs_comm_result, scs_error
-
-    def SyncWritePos(self, scs_id, position, time, speed):
-        txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)]
-        return self.groupSyncWrite.addParam(scs_id, txpacket)
-
-    def RegWritePos(self, scs_id, position, time, speed):
-        txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)]
-        return self.regWriteTxRx(scs_id, SCSCL_GOAL_POSITION_L, len(txpacket), txpacket)
-
-    def RegAction(self):
-        return self.action(BROADCAST_ID)
-
-    def PWMMode(self, scs_id):
-        txpacket = [0, 0, 0, 0]
-        return self.writeTxRx(scs_id, SCSCL_MIN_ANGLE_LIMIT_L, len(txpacket), txpacket)
-
-    def WritePWM(self, scs_id, time):
-        return self.write2ByteTxRx(scs_id, SCSCL_GOAL_TIME_L, self.scs_toscs(time, 10))
-
-    def LockEprom(self, scs_id):
-        return self.write1ByteTxRx(scs_id, SCSCL_LOCK, 1)
-
-    def unLockEprom(self, scs_id):
-        return self.write1ByteTxRx(scs_id, SCSCL_LOCK, 0)
--- a/toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py
+++ b/toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py
@@ -1,7 +0,0 @@
-from setuptools import setup, find_packages
-
-setup(
-    name='STservo_sdk',
-    version='0.1',
-    packages=find_packages(),
-)
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
pimpest	c848bb658d	cleanup old functions in frontend	2026-03-20 11:28:53 +01:00
pimpest	2cc91c1d5d	Added actions endpoint multiple clients can use to simultaneously control actions	2026-03-20 11:23:51 +01:00
pimpest	28b120b88e	Initial webpage and backend	2026-03-17 12:33:12 +01:00