Added basic README

.gitignore

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

.gitmodules

@@ -1,3 +1,6 @@
 [submodule "ext/BehaviorTree.ROS2"]
 	path = ext/BehaviorTree.ROS2
 	url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git
+[submodule "ext/camera_ros"]
+	path = ext/camera_ros
+	url = https://github.com/christianrauch/camera_ros.git

Dockerfile

@@ -1,15 +1,33 @@
-FROM ros:jazzy-ros-base
+FROM ros:jazzy-perception
 
 ENV DEBIAN_FRONTEND=noninteractive
 
 # ---------- System dependencies ----------
-RUN apt-get update && apt-get install -y \
+RUN apt-get update && apt-get upgrade && apt-get install -y \
     python3-colcon-common-extensions \
     python3-rosdep \
+    meson cmake \
     build-essential \
     udev \
     git
 
+# ---------- Libcamera ----------
+
+WORKDIR /extras
+
+RUN apt-get install -y libboost-dev \
+    libgnutls28-dev openssl libtiff5-dev \
+    pybind11-dev \
+    qtbase5-dev libqt5core5a libqt5gui5 libqt5widgets5 \
+    python3-yaml python3-ply python3-jinja2 python3-pip\
+    libglib2.0-dev libgstreamer-plugins-base1.0-dev \
+    build-essential
+
+RUN git clone https://github.com/raspberrypi/libcamera.git \
+    && cd libcamera \
+    && meson setup build --buildtype=release -Dpipelines=rpi/vc4,rpi/pisp -Dipas=rpi/vc4,rpi/pisp -Dv4l2=true -Dgstreamer=enabled -Dtest=false -Dlc-compliance=disabled -Dcam=disabled -Dqcam=disabled -Ddocumentation=disabled -Dpycamera=enabled \
+    && ninja -C build install
+
 # ---------- Initialize rosdep ----------
 RUN rosdep init || true
 RUN rosdep update
@@ -22,13 +40,20 @@ COPY toid_bot_description/package.xml    toid_bot_description/package.xml
 COPY toid_control/package.xml            toid_control/package.xml
 COPY toid_msgs/package.xml               toid_msgs/package.xml
 COPY toid_odometry/package.xml           toid_odometry/package.xml
+COPY toid_lidar/package.xml              toid_lidar/package.xml
+COPY toid_interaction/package.xml        toid_interaction/package.xml
+COPY toid_costmaps/package.xml           toid_costmaps/package.xml
+COPY toid_bt/package.xml                 toid_bt/package.xml
 COPY toid_spinner_controller/package.xml toid_spinner_controller/package.xml
 COPY toid_behaviors/package.xml          toid_behaviors/package.xml
 COPY toid_navigation/package.xml         toid_navigation/package.xml
+COPY toid_vision/package.xml             toid_vision/package.xml
+COPY ext/camera_ros/package.xml          ext/camera_ros/package.xml
+COPY ext/BehaviorTree.ROS2/              ext/BehaviorTree.ROS2/
 
 # ---------- Install dependencies ----------
 RUN . /opt/ros/jazzy/setup.sh && \
-    rosdep install --from-paths ./ --ignore-src -r -y \
+    rosdep install --from-paths ./ --ignore-src -r -y --skip-keys=libcamera \
     && rm -rf /var/lib/apt/lists/*
 
 RUN rm -rf ./*

README.md

@@ -0,0 +1,3 @@
+# Toid - Eurobot platform
+
+This repo contains all the software team Robotoid used for Eurobot 2026.

docker-compose.yaml

@@ -1,17 +1,37 @@
 services:
   toid:
     image: localhost:5000/toid 
+    build: .
     container_name: toid
 
     privileged: true
     network_mode: host
 
     volumes:
-      - ./:/ros_ws/src
+      - ./:/ros_ws
       - /dev/:/dev/
       - /sys/:/sys/
+      - /run/udev/:/run/udev/
 
     entrypoint: ["sleep","infinity"]
 
     profiles:
       - base
+
+  toid_forever:
+    image: localhost:5000/toid 
+    build: .
+    container_name: toid
+
+    restart: unless-stopped
+
+    privileged: true
+    network_mode: host
+
+    volumes:
+      - ./:/ros_ws
+      - /dev/:/dev/
+      - /sys/:/sys/
+      - /run/udev/:/run/udev/
+
+    entrypoint: ["bash", "-c", "source install/setup.bash && ros2 launch toid_navigation main.py use_lidar:=False use_mock:=False"]

firmware/base/src/config.h

@@ -24,7 +24,8 @@
 #define ENCODER_LEFT_PIN_B  13
 #define ENCODER_CPR         3840
 
-#define WHEEL_RADIUS     0.0300
+//#define WHEEL_RADIUS     (0.0300 * 1.01483541)
-#define WHEEL_SEPARATION 0.264
+#define WHEEL_RADIUS     0.028
+#define WHEEL_SEPARATION 0.271
 #define TIMER_CYCLE_US   1000
 //======================================================

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.000,
+    .left_gain = 1.0,  
-    .right_gain = 1.0000
+    .right_gain = 1.0,
 };
 
 void update_pos_cb() {

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: 1.325
+          x: 0.0
-          y: 0.045
+          y: 0.0
-          theta: -1.57079632679489661922
+          theta: 0.0
           backwards: ${SMER:-0}"
         ;;
     "pravo") 

scripts/toid_base_build.sh

@@ -3,3 +3,4 @@
 source /opt/ros/jazzy/setup.bash
 
 colcon build --packages-up-to-regex "toid_(?!bt)"
+colcon build --packages-up-to-regex "camera_ros"

toid_behaviors/CMakeLists.txt

@@ -32,6 +32,8 @@ set(
   src/simple_move.cpp
   src/simple_rotate.cpp
   src/simple_translate_x.cpp
+  src/approach_acorns.cpp
+  src/rotate_acorns.cpp
 )
 
 find_package(ament_cmake REQUIRED)

toid_behaviors/include/toid_behaviors/approach_acorns.hpp

@@ -0,0 +1,91 @@
+#pragma once
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "toid_behaviors/scl.hpp"
+#include "toid_behaviors/simple_move.hpp"
+#include "toid_behaviors/rolling_average.hpp"
+#include "toid_msgs/action/simple_move_coords.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+
+namespace toid
+{
+using MoveAction = toid_msgs::action::SimpleMoveCoords;
+using PoseStamped = geometry_msgs::msg::PoseStamped;
+using namespace nav2_behaviors;
+
+class ApproachAcorns : public SimpleMove<MoveAction>
+{
+public:
+  ApproachAcorns();
+  ~ApproachAcorns();
+
+  void configureCB() override;
+
+  void activateCB() override;
+  void deactivateCB() override;
+
+  ResultStatus onStart(
+    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);
+
+  void acorn_position_cb(PoseStamped::ConstSharedPtr msg);
+
+  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;
+  }
+
+protected:
+  SmoothControlLaw scl_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr acorn_pose_sub_;
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr target_pose_pub_;
+  std::shared_mutex mutex_;
+  RollingAverage avg_ = RollingAverage(10);
+
+  //Goal
+  geometry_msgs::msg::Pose new_target_pose_;
+  double new_target_angle_;
+
+  geometry_msgs::msg::Pose target_pose_;
+  geometry_msgs::msg::Pose initial_pose_;
+  double target_angle_;
+  double target_sign_;
+  bool backwards_;
+  unsigned char mode_;
+
+  //State
+  double last_speed_;
+  rclcpp::Time last_seen_;
+  double distance_;
+
+  //Config
+  double max_vel_accel_;
+  double max_vel_decel_;
+  double max_vel_speed_;
+  double min_vel_speed_;
+
+  double max_angular_speed_;
+
+  double kp_;
+  double k_phi_;
+  double k_delta_;
+  double beta_;
+  double lambda_;
+  double slowdown_radius_;
+  bool debug_marker_;
+};
+
+}  // namespace toid

toid_behaviors/include/toid_behaviors/move_coords.hpp

@@ -23,23 +23,31 @@ 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::NONE;
+    return nav2_core::CostmapInfoType::LOCAL;
   }
 
 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_;

toid_behaviors/include/toid_behaviors/rolling_average.hpp

@@ -0,0 +1,67 @@
+#pragma once
+
+#include <algorithm>
+#include <vector>
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+class RollingAverage
+{
+public:
+  using Pose2D = std::tuple<double, double, double>;
+  RollingAverage(size_t size = 0) : poses_(size), size_(size) {}
+
+  Pose2D push(geometry_msgs::msg::Pose & pose)
+  {
+    if(size_ == 0) {
+        return {};
+    }
+    if (size_ == data_count_) {
+      accum_x_ -= poses_[front_idx_].position.x;
+      accum_y_ -= poses_[front_idx_].position.y;
+      accum_theta_ -= tf2::getYaw(poses_[front_idx_].orientation);
+      front_idx_ += 1;
+      front_idx_ %= size_;
+      data_count_--;
+    }
+
+    poses_[back_idx_] = pose;
+
+    accum_x_ += poses_[back_idx_].position.x;
+    accum_y_ += poses_[back_idx_].position.y;
+    accum_theta_ += tf2::getYaw(poses_[back_idx_].orientation);
+
+    back_idx_ += 1;
+    back_idx_ %= size_;
+    data_count_++;
+
+    return {accum_x_ / data_count_, accum_y_ / data_count_, accum_theta_ / data_count_};
+  }
+
+  void reset() {
+    data_count_ = 0;
+    front_idx_  = 0;
+    back_idx_   = 0;
+    accum_x_ = 0;
+    accum_y_ = 0;
+    accum_theta_ = 0;
+  }
+
+private:
+  std::vector<geometry_msgs::msg::Pose> poses_;
+  const size_t size_;
+  size_t front_idx_ = 0;
+  size_t back_idx_ = 0;
+  size_t data_count_ = 0;
+
+  double accum_x_ = 0;
+  double accum_y_ = 0;
+  double accum_theta_ = 0;
+};
+
+}  // namespace toid

toid_behaviors/include/toid_behaviors/rotate_acorns.hpp

@@ -0,0 +1,80 @@
+#pragma once
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "toid_behaviors/scl.hpp"
+#include "toid_behaviors/simple_move.hpp"
+#include "toid_behaviors/rolling_average.hpp"
+#include "toid_msgs/action/simple_move_coords.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+
+namespace toid
+{
+using RotateAction = toid_msgs::action::SimpleRotate;
+using PoseStamped = geometry_msgs::msg::PoseStamped;
+using namespace nav2_behaviors;
+
+class RotateAcorns : public SimpleMove<RotateAction>
+{
+public:
+  RotateAcorns();
+  ~RotateAcorns();
+
+  void configureCB() override;
+
+  void activateCB() override;
+  void deactivateCB() override;
+
+  ResultStatus onStart(
+    const std::shared_ptr<const RotateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+    const geometry_msgs::msg::Twist & vel) override;
+
+  bool collisionDetection(
+    const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose);
+
+  void acorn_position_cb(PoseStamped::ConstSharedPtr msg);
+
+  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;
+  }
+
+  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:
+  SmoothControlLaw scl_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr acorn_pose_sub_;
+  double new_target_angle_;
+  std::shared_mutex mutex_;
+  RollingAverage avg_ = RollingAverage(10);
+
+
+  //Goal
+  double target_angle_;
+  double min_turn_angle_;
+  double initial_direction_;
+  unsigned char mode_;
+
+  //State
+  double angular_speed_;
+  double last_angle_;
+  double last_time_;
+  double distance_ = 1.0;
+
+  //Config
+  double max_angular_speed_;
+  double min_angular_speed_;
+  double max_angular_accel_;
+  double max_angular_decel_;
+  double lookahead_;
+};
+
+}  // namespace toid

toid_behaviors/include/toid_behaviors/scl.hpp

@@ -19,13 +19,17 @@ public:
   double v_angular_max = 2.0;
 
   void egocentric_polar(
-    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
+    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
-    double & r, double & phi, double & delta);
+    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

toid_behaviors/include/toid_behaviors/simple_move.hpp

@@ -6,6 +6,7 @@
 #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
 {
@@ -14,6 +15,8 @@ template <typename ActionT>
 class SimpleMove : public nav2_behaviors::TimedBehavior<ActionT>
 {
 public:
+  using Rival = toid_msgs::msg::Rival;
+
   virtual void configureCB() {}
   virtual void cleanupCB() {}
 
@@ -34,6 +37,18 @@ 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_);
@@ -52,12 +67,17 @@ 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();
   }
 
@@ -105,12 +125,55 @@ public:
     return r;
   }
 
+  bool check_rival_collision(geometry_msgs::msg::Pose2D & pose)
+  {
+    if (!rivals_ || (this->clock_->now() - rivals_->header.stamp ) > rclcpp::Duration::from_seconds(1.0)) {
+      return false;
+    }
+    const double cosp = std::cos(pose.theta);
+    const double sinp = std::sin(pose.theta);
+    int i = 0;
+    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;
+
+      local_rival.x -= 0.105;
+
+      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);
+      RCLCPP_DEBUG(this->logger_, "Distance to rival %d: %lf", i++, sdf-rival_radius_);
+      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.22;
+
+  Rival::SharedPtr rivals_;
+  rclcpp::Subscription<Rival>::SharedPtr rivals_sub_;
 };
 
 }  // namespace toid

toid_behaviors/include/toid_behaviors/simple_rotate.hpp

@@ -2,6 +2,7 @@
 
 #include "toid_behaviors/simple_move.hpp"
 #include "toid_msgs/action/simple_rotate.h"
+#include "toid_msgs/msg/rival.hpp"
 
 namespace toid
 {
@@ -24,16 +25,24 @@ public:
     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 {
+  virtual nav2_core::CostmapInfoType getResourceInfo() override
-    return nav2_core::CostmapInfoType::NONE;
+  {
+    return nav2_core::CostmapInfoType::LOCAL;
   }
 
-protected:
+  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_;
@@ -45,6 +54,7 @@ protected:
   double min_angular_speed_;
   double max_angular_accel_;
   double max_angular_decel_;
+  double lookahead_;
 };
 
 }  // namespace toid

toid_behaviors/src/approach_acorns.cpp

@@ -0,0 +1,311 @@
+#include "toid_behaviors/approach_acorns.hpp"
+
+#include "angles/angles.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+ApproachAcorns::ApproachAcorns() : SimpleMove<MoveAction>() {}
+ApproachAcorns::~ApproachAcorns() {}
+
+void ApproachAcorns::configureCB()
+{
+  auto node = node_.lock();
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_vel_accel", rclcpp::ParameterValue(2.0));
+  node->get_parameter(behavior_name_ + ".max_vel_accel", max_vel_accel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_vel_decel", rclcpp::ParameterValue(1.0));
+  node->get_parameter(behavior_name_ + ".max_vel_decel", max_vel_decel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_vel_speed", rclcpp::ParameterValue(0.4));
+  node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".min_vel_speed", rclcpp::ParameterValue(0.10));
+  node->get_parameter(behavior_name_ + ".min_vel_speed", min_vel_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_speed", rclcpp::ParameterValue(1.0));
+  node->get_parameter(behavior_name_ + ".max_angular_speed", max_angular_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".kp", rclcpp::ParameterValue(5.0));
+  node->get_parameter(behavior_name_ + ".kp", kp_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".kphi", rclcpp::ParameterValue(5.0));
+  node->get_parameter(behavior_name_ + ".kphi", k_phi_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".kdelta", rclcpp::ParameterValue(2.0));
+  node->get_parameter(behavior_name_ + ".kdelta", k_delta_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".beta", rclcpp::ParameterValue(0.4));
+  node->get_parameter(behavior_name_ + ".beta", beta_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".lambda", rclcpp::ParameterValue(2.0));
+  node->get_parameter(behavior_name_ + ".lambda", lambda_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".slow_down_radius", rclcpp::ParameterValue(0.20));
+  node->get_parameter(behavior_name_ + ".slow_down_radius", slowdown_radius_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".debug_marker", rclcpp::ParameterValue(false));
+  node->get_parameter(behavior_name_ + ".debug_marker", debug_marker_);
+
+  target_pose_pub_ = node->create_publisher<visualization_msgs::msg::Marker>("marker", 1);
+}
+
+void ApproachAcorns::activateCB()
+{
+  auto node = node_.lock();
+  using namespace std::placeholders;
+  acorn_pose_sub_ = node->create_subscription<PoseStamped>(
+    "closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&ApproachAcorns::acorn_position_cb, this, _1));
+  target_pose_pub_->on_activate();
+  distance_ = 1.0;
+}
+
+void ApproachAcorns::deactivateCB()
+{
+  acorn_pose_sub_.reset();
+  target_pose_pub_->on_deactivate();
+}
+
+void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
+{
+  geometry_msgs::msg::PoseStamped pose_local;
+  geometry_msgs::msg::PoseStamped pose_global;
+  try {
+    pose_local = tf_->transform(*msg, robot_base_frame_);
+    pose_global = tf_->transform(*msg, global_frame_);
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]...");
+    return;
+  }
+
+  double x = pose_local.pose.position.x;
+  double y = pose_local.pose.position.y;
+  if (std::sqrt(x * x + y * y) > distance_ + 0.005) {
+    return;
+  }
+
+  // Smooth out approach
+  if (x * x + y * y < 0.42 * 0.42) {
+    return;
+  }
+
+  double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation));
+
+  if (tf2::getYaw(pose_local.pose.orientation) > 0) {
+    pose_global.pose.position.x += std::cos(yaw) * 0.01 - std::sin(yaw) * 0.35;
+    pose_global.pose.position.y += std::sin(yaw) * 0.01 + std::cos(yaw) * 0.35;
+  } else {
+    pose_global.pose.position.x += std::cos(yaw) * -0.01 - std::sin(yaw) * -0.35;
+    pose_global.pose.position.y += std::sin(yaw) * -0.01 + std::cos(yaw) * -0.35;
+  }
+
+  std::lock_guard _lock(mutex_);
+
+  const double dx = pose_global.pose.position.x - initial_pose_.position.x;
+  const double dy = pose_global.pose.position.y - initial_pose_.position.y;
+  double yaw_to_goal = std::atan2(dy,dx);
+
+  tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw_to_goal), pose_global.pose.orientation);
+  yaw = yaw_to_goal;
+  auto [a, b, c] = avg_.push(pose_global.pose);
+  distance_ = x * x + y * y;
+  new_target_pose_.position.x = a;
+  new_target_pose_.position.y = b;
+  tf2::convert(tf2::Quaternion::createFromRPY(0, 0, c), new_target_pose_.orientation);
+  new_target_angle_ = c;
+
+  if (debug_marker_) {
+    visualization_msgs::msg::Marker marker;
+    marker.lifetime.sec = 1.0;
+    marker.header = pose_global.header;
+    marker.pose = new_target_pose_;
+    marker.type = visualization_msgs::msg::Marker::ARROW;
+    marker.scale.x = 0.02;
+    marker.scale.y = 0.02;
+    marker.scale.z = 0.02;
+    marker.color.a = 1.0;
+    marker.color.r = 1.0;
+    marker.color.g = 0;
+    marker.color.b = 0;
+    target_pose_pub_->publish(marker);
+  }
+}
+
+ResultStatus ApproachAcorns::onStart(
+  const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+  const geometry_msgs::msg::Twist & vel)
+{
+  std::lock_guard _lock(mutex_);
+  distance_ = 1.0;
+  new_target_pose_.position.x = command->x;
+  new_target_pose_.position.y = command->y;
+
+  backwards_ = command->backwards;
+  initial_pose_ = pose;
+
+  new_target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta));
+  new_target_angle_ = command->theta;
+  target_sign_ = backwards_ ? -1.0 : 1.0;
+  max_vel_speed_ = command->max_speed;
+
+  avg_.reset();
+
+  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_;
+
+  last_speed_ = vel.angular.x;
+
+  return ResultStatus{Status::SUCCEEDED};
+}
+
+double ApproachAcorns::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 ApproachAcorns::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 ApproachAcorns::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 ApproachAcorns::updateVel(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
+  geometry_msgs::msg::Twist & out_vel)
+{
+  {
+    std::lock_guard _lock(mutex_);
+    target_pose_ = new_target_pose_;
+    target_angle_ = new_target_angle_;
+  }
+
+  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 (dist_left <= 0.001 && std::fabs(angle_dist) <= 0.030) {
+    out_vel.linear.x = 0;
+    out_vel.angular.z = 0;
+    return ResultStatus{Status::SUCCEEDED};
+  }
+
+  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.001 && 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_);
+    last_speed_ = out_vel.linear.x;
+    return ResultStatus{Status::RUNNING};
+  }
+  
+  if (dist_left <= 0.001) {
+    out_vel.linear.x = 0;
+    out_vel.angular.z = std::clamp(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_);
+
+  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};
+}
+
+}  // namespace toid
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::ApproachAcorns, nav2_core::Behavior);

toid_behaviors/src/move_coords.cpp

@@ -69,43 +69,87 @@ 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_);
   }
 
-  scl.k_phi = k_phi_;
+  mode_ = command->mode;
-  scl.k_delta = k_delta_;
+
-  scl.bbeta = beta_;
+  scl_.k_phi = k_phi_;
-  scl.lam = lambda_;
+  scl_.k_delta = k_delta_;
-  scl.slowdown_radius = slowdown_radius_;
+  scl_.bbeta = beta_;
-  scl.v_angular_max = max_angular_speed_;
+  scl_.lam = lambda_;
-  scl.v_linear_min = min_vel_speed_;
+  scl_.slowdown_radius = slowdown_radius_;
-  scl.v_linear_max = max_vel_speed_;
+  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)
 {
-  const double current_yaw = tf2::getYaw(pose.orientation);
+  double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_);
-  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;
@@ -113,30 +157,48 @@ ResultStatus MoveCoords::updateVel(
     return ResultStatus{Status::SUCCEEDED};
   }
 
-  double vel = max_vel_speed_;
+  const double current_yaw = tf2::getYaw(pose.orientation);
-  double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
+  double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
-  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 = std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+    out_vel.linear.x = velocityTarget(dist_left);
-    out_vel.angular.z = std::clamp(target_sign_ * kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
+    out_vel.angular.z = std::clamp(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_ * std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+  scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
-  scl.calculate_vel(target_pose_, pose, out_vel, backwards_);
+  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, "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);

toid_behaviors/src/rotate_acorns.cpp

@@ -0,0 +1,234 @@
+#include "toid_behaviors/rotate_acorns.hpp"
+
+#include <cmath>
+
+#include "angles/angles.h"
+#include "tf2/convert.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+RotateAcorns::RotateAcorns() : SimpleMove<RotateAction>() {}
+RotateAcorns::~RotateAcorns() {}
+
+void RotateAcorns::configureCB()
+{
+  auto node = node_.lock();
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_vel", rclcpp::ParameterValue(2.0));
+
+  node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".min_angular_vel", rclcpp::ParameterValue(0.1));
+  node->get_parameter(behavior_name_ + ".min_angular_vel", min_angular_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_accel", rclcpp::ParameterValue(4.0));
+  node->get_parameter(behavior_name_ + ".max_angular_accel", max_angular_accel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(0.5));
+  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_);
+}
+
+
+void RotateAcorns::activateCB()
+{
+  auto node = node_.lock();
+  using namespace std::placeholders;
+  acorn_pose_sub_ = node->create_subscription<PoseStamped>(
+    "closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&RotateAcorns::acorn_position_cb, this, _1));
+  distance_ = 1.0;
+}
+
+void RotateAcorns::deactivateCB() { 
+    acorn_pose_sub_.reset(); 
+}
+
+void RotateAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
+{
+  geometry_msgs::msg::PoseStamped pose_local;
+  geometry_msgs::msg::PoseStamped pose_global;
+  try {
+    pose_local = tf_->transform(*msg, robot_base_frame_);
+    pose_global = tf_->transform(*msg, global_frame_);
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]...");
+    return;
+  }
+
+  double x = pose_local.pose.position.x;
+  double y = pose_local.pose.position.y;
+  if (x * x + y * y > distance_ + 0.005) {
+    return;
+  }
+
+  double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation));
+
+  if(tf2::getYaw(pose_local.pose.orientation) > 0) {
+    yaw += M_PI;
+  }
+
+  yaw += M_PI/2;
+
+
+  pose_global.pose.position.x += std::cos(yaw) * 0.0 - std::sin(yaw) * - 0.005;
+  pose_global.pose.position.y += std::sin(yaw) * 0.0 + std::cos(yaw) * - 0.005;
+  tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation);
+
+  yaw = angles::normalize_angle(yaw);
+
+  std::lock_guard _lock(mutex_);
+  auto[a,b,c] = avg_.push(pose_global.pose);
+  distance_ = x * x + y * y;
+  new_target_angle_ = c;
+  
+}
+
+ResultStatus RotateAcorns::onStart(
+  const std::shared_ptr<const RotateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+  const geometry_msgs::msg::Twist & vel)
+{
+  std::lock_guard _lock(mutex_);
+  target_angle_ = new_target_angle_;
+  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;
+  distance_ = 1.0;
+  avg_.reset();
+
+  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;
+  last_time_ = clock_->now().seconds();
+  return ResultStatus{Status::SUCCEEDED};
+}
+
+void RotateAcorns::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 RotateAcorns::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_decel_ * 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 RotateAcorns::updateVel(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
+  geometry_msgs::msg::Twist & out_vel)
+{
+  {
+    std::lock_guard _lock(mutex_);
+    target_angle_ = new_target_angle_;
+  }
+  const double current_yaw = tf2::getYaw(pose.orientation);
+  double min_turn_angle = min_turn_angle_;
+  double angular_speed = angular_speed_;
+  double err, sign;
+
+  calc_err_and_sign(last_angle_, current_yaw, min_turn_angle, err, sign);
+
+  if (!(mode_ & RotateAction::Goal::IGNORE_OBSTACLES)) {
+    err = check_space(pose, err, sign);
+  }
+
+  double speed = 0.0;
+
+  if (err != 0.0) {
+    speed = calc_speed(err, sign, angular_speed);
+  }
+
+  if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.004) {
+    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 RotateAcorns::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"
+PLUGINLIB_EXPORT_CLASS(toid::RotateAcorns, nav2_core::Behavior);

toid_behaviors/src/scl.cpp

@@ -3,20 +3,21 @@
 #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));
 
@@ -44,8 +45,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,
+  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
-  double & phi, double & delta)
+  double & r, double & phi, double & delta)
 {
   const double dx = target.position.x - current.position.x;
   const double dy = target.position.y - current.position.y;
@@ -59,4 +60,20 @@ 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

toid_behaviors/src/simple_rotate.cpp

@@ -4,6 +4,7 @@
 
 #include "angles/angles.h"
 #include "tf2/convert.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 namespace toid
 {
@@ -30,6 +31,10 @@ 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(
@@ -40,12 +45,19 @@ 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;
@@ -53,43 +65,108 @@ 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);
-  const double angle_change = angles::shortest_angular_distance(last_angle_ , current_yaw);
+  double min_turn_angle = min_turn_angle_;
-  last_angle_ = current_yaw;
+  double angular_speed = angular_speed_;
+  double err, sign;
 
-  double err = angles::shortest_angular_distance(current_yaw, target_angle_);
+  calc_err_and_sign(last_angle_, current_yaw, min_turn_angle, err, sign);
-  double sign = (err < 0)? -1.0 : 1.0;
-  err = std::abs(err);
 
-  if (min_turn_angle_ > 0.0) {
+  if (!(mode_ & RotateAction::Goal::IGNORE_OBSTACLES)) {
-    min_turn_angle_ = std::max(0.0, min_turn_angle_ - initial_direction_ * angle_change);
+    err = check_space(pose, err, sign);
-    err = std::max( initial_direction_ * sign * err, 0.0);
-    err = std::max(min_turn_angle_, err);
-    sign = initial_direction_;
   }
 
-  const double upper_vel_ = angular_speed_ + max_angular_accel_ * control_duration_;
+  double speed = 0.0;
-  const double lower_vel_ = angular_speed_ - max_angular_accel_ * control_duration_;
 
-  const double speed_until_overshoot =
+  if (err != 0.0) {
-    std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
+    speed = calc_speed(err, sign, angular_speed);
-
+  }
-  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"

toid_behaviors/toid_behaviors.xml

@@ -9,5 +9,11 @@
         <class type="toid::MoveCoords" base_class_type="nav2_core::Behavior">
             <description></description>
         </class>
+        <class type="toid::ApproachAcorns" base_class_type="nav2_core::Behavior">
+            <description></description>
+        </class>
+        <class type="toid::RotateAcorns" base_class_type="nav2_core::Behavior">
+            <description></description>
+        </class>
     </library>
 </class_libraries>

toid_bot_description/rviz/default.rviz

@@ -1,13 +1,14 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 78
+    Help Height: 138
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
         - /Status1
+        - /Pose1
       Splitter Ratio: 0.5
-    Tree Height: 542
+    Tree Height: 732
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -74,6 +75,14 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         drivewhl_l_link:
           Alpha: 1
           Show Axes: false
@@ -118,6 +127,10 @@ Visualization Manager:
           Value: true
         base_link:
           Value: true
+        camera:
+          Value: true
+        camera_base:
+          Value: true
         drivewhl_l_link:
           Value: true
         drivewhl_r_link:
@@ -134,9 +147,8 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        base_link:
         base_footprint:
-            {}
+          base_link:
             drivewhl_l_link:
               {}
             drivewhl_r_link:
@@ -147,8 +159,31 @@ Visualization Manager:
               {}
             right_caster:
               {}
+          camera_base:
+            camera:
+              {}
       Update Interval: 0
       Value: true
+    - Alpha: 1
+      Axes Length: 0.10000000149011612
+      Axes Radius: 0.009999999776482582
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.10000000149011612
+      Head Radius: 0.029999999329447746
+      Name: Pose
+      Shaft Length: 0.10000000149011612
+      Shaft Radius: 0.009999999776482582
+      Shape: Axes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /closest_acorn
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -195,7 +230,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/XYOrbit
-      Distance: 1.59040105342865
+      Distance: 1.2598285675048828
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -210,18 +245,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6053992509841919
+      Pitch: 0.015398791059851646
       Target Frame: <Fixed Frame>
       Value: XYOrbit (rviz_default_plugins)
-      Yaw: 1.088563323020935
+      Yaw: 0.09356015175580978
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 846
+  Height: 1250
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016c000002acfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002ac000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002ac000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002de00fffffffb0000000800540069006d0065010000000000000450000000000000000000000229000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000027a000003d8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000003d80000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b000003d8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000003d80000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007620000005efc0100000002fb0000000800540069006d00650100000000000007620000048700fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000003d800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -230,6 +265,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1200
+  Width: 1890
   X: 60
-  Y: 60
+  Y: 64

toid_bot_description/src/toid_bot_control.xacro

@@ -1,12 +1,13 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="toid_control" params="name:=toid_control use_mock_hardware:=^|true">
+  <xacro:macro name="toid_control" params="name:=toid_control use_mock_hardware:=^|true serial_port:=/dev/ttyACM0">
 
     <ros2_control name="${name}" type="system">
       <xacro:unless value="${use_mock_hardware}">
         <hardware>
             <plugin>toid_control/StepperInterface</plugin>
+            <param name="device_path">${serial_port}</param>
         </hardware>
       </xacro:unless>
       <xacro:if value="${use_mock_hardware}">

toid_bot_description/src/toid_bot_description.urdf

@@ -2,6 +2,7 @@
 <robot name="toid_bot" xmlns:xacro="http://ros.org/wiki/xacro">
 
 <xacro:arg name="use_mock" default="true" />
+<xacro:arg name="serial_port" default="/dev/ttyACM0" />
 
 <xacro:property name="base_width" value="0.30"/>
 <xacro:property name="base_length" value="0.30"/>
@@ -11,7 +12,7 @@
 <xacro:property name="wheel_width" value="0.03"/>
 
 <xacro:property name="wheel_zoff" value="0.03"/>
-<xacro:property name="wheel_xoff" value="-0.07"/>
+<xacro:property name="wheel_xoff" value="-0.105"/>
 <xacro:property name="wheel_inset" value="0.01"/>
 
 <xacro:property name="caster_radius" value="0.02"/>
@@ -22,7 +23,11 @@
 
 <xacro:property name="lidar_radius" value="0.03"/>
 <xacro:property name="lidar_height" value="0.02"/>
-<xacro:property name="lidar_xoff" value="0.09"/>
+<xacro:property name="lidar_xoff" value="0.00"/>
+
+<xacro:property name="camera_xoff" value="0.07369"/>
+<xacro:property name="camera_zoff" value="0.16664"/>
+<xacro:property name="camera_pitch" value="${-105 * pi/180}"/>
 
 <xacro:property name="base_zoff" value="${-wheel_zoff + wheel_radius + base_height/2}"/>
 
@@ -39,6 +44,9 @@
 
 <link name="base_footprint"/>
 
+<link name="camera" />
+
+
 <link name="lidar">
     <visual>
         <geometry>
@@ -56,10 +64,16 @@
     <origin xyz="${-wheel_xoff} 0.0 ${base_zoff}" rpy="0 0 0"/>
 </joint>
 
+<joint name="camera_base_joint" type="fixed">
+    <parent link="base_footprint"/>
+    <child link="camera"/>
+    <origin xyz="${camera_xoff} 0 ${camera_zoff}" rpy="${camera_pitch} 0 ${-pi/2}"/>
+</joint>
+
 <joint name="lidar_joint" type="fixed">
     <parent link="base_link"/>
     <child link="lidar"/>
-    <origin xyz="${lidar_xoff} 0 ${(base_height + lidar_height)/2}" rpy="0 0 0"/>
+    <origin xyz="${lidar_xoff} 0 ${(base_height + lidar_height)/2}" rpy="0 0 ${pi}"/>
 </joint>
 
 
@@ -112,6 +126,6 @@
 <xacro:cstr prefix="right" y_reflect="-1" />
 
 <xacro:include filename="$(find toid_bot_description)/src/toid_bot_control.xacro"/>
-<xacro:toid_control name="toid_bot_control" use_mock_hardware="$(arg use_mock)"/>
+<xacro:toid_control name="toid_bot_control" use_mock_hardware="$(arg use_mock)" serial_port="$(arg serial_port)"/>
 
 </robot>

toid_bt/behavior_trees/behavior1.xml

@@ -1,17 +1,367 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
+  <BehaviorTree ID="pickup_crates">
+    <Seq1 service_name=""/>
+  </BehaviorTree>
+
+  <BehaviorTree ID="seq">
+    <Sequence>
+      <InPose timeout="1.000000">
+        <Sleep msec="999999"/>
+      </InPose>
+      <Seq1 service_name=""/>
+      <Seq2 text="1010"
+            service_name=""/>
+      <Seq3 service_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="seq1">
+    <Sequence>
+      <ZeroOdom service_name=""/>
+      <SetInitialPose y="0.805"
+                      theta="-90"
+                      frame_id="map"
+                      x="1.325"
+                      topic_name="__default__placeholder__"/>
+      <WaitPullPin action_name=""/>
+      <Timeout msec="100000">
+        <Sequence>
+          <InPose timeout="1.000000">
+            <MovePointSimple x="1.325"
+                             y="0.200"
+                             theta="-90"
+                             max_speed="0.200000"
+                             backwards="false"
+                             action_name=""/>
+          </InPose>
+          <Seq1 service_name=""/>
+          <Seq2 text=""
+                service_name=""/>
+          <MovePointSimple x="1.325"
+                           y="-0.05"
+                           theta="-90"
+                           max_speed="0.200000"
+                           backwards="false"
+                           action_name=""/>
+          <Seq3 service_name=""/>
+          <MovePointSimple x="1.325"
+                           y="0.805"
+                           theta="-90"
+                           max_speed="0.200000"
+                           backwards="true"
+                           action_name=""/>
+        </Sequence>
+      </Timeout>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="seq2">
+    <Sequence>
+      <ZeroOdom service_name=""/>
+      <SetInitialPose y="0.955"
+                      theta="-90"
+                      frame_id="map"
+                      x="1.330"
+                      topic_name="__default__placeholder__"/>
+      <MovePointSimple x="1.330"
+                       y="0.805"
+                       theta="-90"
+                       max_speed="0.300000"
+                       backwards="false"
+                       action_name=""/>
+      <WaitPullPin action_name=""/>
+      <Timeout msec="100000">
+        <Sequence>
+          <InPose timeout="1.000000">
+            <MovePointSimple x="1.33"
+                             y="0.354"
+                             theta="-90"
+                             max_speed="0.300000"
+                             backwards="false"
+                             action_name=""/>
+          </InPose>
+          <Seq1 service_name=""/>
+          <Parallel failure_count="1"
+                    success_count="-1">
+            <Seq2 text=""
+                  service_name=""/>
+            <MovePointSimple x="0.795"
+                             y="-0.2"
+                             theta="180"
+                             max_speed="0.500000"
+                             backwards="false"
+                             action_name=""/>
+          </Parallel>
+          <Parallel failure_count="1"
+                    success_count="-1">
+            <Seq3 service_name=""/>
+            <Sequence>
+              <Sleep msec="500"/>
+              <TranslateX x="-0.3"
+                          max_speed="0.000000"
+                          action_name=""/>
+              <RotateTowards x="1.33"
+                             y="0.354"
+                             backwards="true"
+                             max_speed="2.000000"
+                             min_angle="0.000000"
+                             action_name=""/>
+            </Sequence>
+          </Parallel>
+          <MovePointSimple x="1.330"
+                           y="0.354"
+                           theta="-90"
+                           max_speed="0.400000"
+                           backwards="true"
+                           action_name=""/>
+          <MovePointSimple x="1.330"
+                           y="-0.52"
+                           theta="-90"
+                           max_speed="0.000000"
+                           backwards="false"
+                           action_name=""/>
+          <Seq1 service_name=""/>
+          <ParallelAll max_failures="1">
+            <Seq2 text=""
+                  service_name=""/>
+            <MovePointSimple x="1.33"
+                             y="-0.2"
+                             theta="-90"
+                             max_speed="0.200000"
+                             backwards="false"
+                             action_name=""/>
+          </ParallelAll>
+          <Parallel failure_count="1"
+                    success_count="-1">
+            <Seq3 service_name=""/>
+            <Sequence>
+              <Sleep msec="500"/>
+              <MovePointSimple x="1.33"
+                               y="0.354"
+                               theta="-90"
+                               max_speed="0.400000"
+                               backwards="true"
+                               action_name=""/>
+              <MovePointSimple x="1.37"
+                               y="0.48"
+                               theta="-70"
+                               max_speed="0.400000"
+                               backwards="true"
+                               action_name=""/>
+            </Sequence>
+          </Parallel>
+          <RotateTowards x="1.003"
+                         y="-0.029"
+                         backwards="false"
+                         max_speed="1.300000"
+                         min_angle="0.000000"
+                         action_name=""/>
+          <MovePointSimple x="1.003"
+                           y="-0.029"
+                           theta="-90"
+                           max_speed="0.400000"
+                           backwards="false"
+                           action_name=""/>
+        </Sequence>
+      </Timeout>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="seq2_yellow">
+    <Sequence>
+      <ZeroOdom service_name=""/>
+      <SetInitialPose y="0.806"
+                      theta="-90"
+                      frame_id="map"
+                      x="-1.328"
+                      topic_name="__default__placeholder__"/>
+      <WaitPullPin action_name=""/>
+      <Timeout msec="100000">
+        <Sequence>
+          <InPose timeout="1.000000">
+            <MovePointSimple x="-1.328"
+                             y="0.354"
+                             theta="-90"
+                             max_speed="0.300000"
+                             backwards="false"
+                             action_name=""/>
+          </InPose>
+          <Seq1 service_name=""/>
+          <Parallel failure_count="1"
+                    success_count="-1">
+            <Seq2 text=""
+                  service_name=""/>
+            <MovePointSimple x="-0.795"
+                             y="-0.2"
+                             theta="0"
+                             max_speed="0.500000"
+                             backwards="false"
+                             action_name=""/>
+          </Parallel>
+          <Parallel failure_count="1"
+                    success_count="-1">
+            <Seq3 service_name=""/>
+            <Sequence>
+              <Sleep msec="500"/>
+              <TranslateX x="-0.3"
+                          max_speed="0.000000"
+                          action_name=""/>
+              <RotateTowards x="-1.328"
+                             y="0.354"
+                             backwards="true"
+                             max_speed="2.000000"
+                             min_angle="0.000000"
+                             action_name=""/>
+            </Sequence>
+          </Parallel>
+          <MovePointSimple x="-1.328"
+                           y="0.354"
+                           theta="-90"
+                           max_speed="0.400000"
+                           backwards="true"
+                           action_name=""/>
+          <Seq1 service_name=""/>
+          <ParallelAll max_failures="1">
+            <Seq2 text=""
+                  service_name=""/>
+            <MovePointSimple x="-1.328"
+                             y="-0.2"
+                             theta="-90"
+                             max_speed="0.200000"
+                             backwards="false"
+                             action_name=""/>
+          </ParallelAll>
+          <Parallel failure_count="1"
+                    success_count="-1">
+            <Seq3 service_name=""/>
+            <Sequence>
+              <Sleep msec="500"/>
+              <MovePointSimple x="-1.328"
+                               y="0.354"
+                               theta="-90"
+                               max_speed="0.400000"
+                               backwards="true"
+                               action_name=""/>
+            </Sequence>
+          </Parallel>
+          <MovePointSimple x="-1.328"
+                           y="0.806"
+                           theta="-90"
+                           max_speed="0.400000"
+                           backwards="true"
+                           action_name=""/>
+        </Sequence>
+      </Timeout>
+    </Sequence>
+  </BehaviorTree>
+
   <BehaviorTree ID="test_1">
     <Sequence>
-      <RotateSimple angle="90"
+      <DetectStuck timeout="1.000000">
-                    min_angle="0.000000"
+        <RotateTowards x="0.4"
-                    action_name=""/>
+                       y="0.0"
-      <Sleep msec="1000"/>
+                       backwards="false"
-      <RotateSimple angle="0"
+                       max_speed="0.000000"
                        min_angle="0.000000"
                        action_name=""/>
+      </DetectStuck>
     </Sequence>
   </BehaviorTree>
 
   <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Decorator ID="DetectStuck">
+      <input_port name="timeout"
+                  default="1.000000"
+                  type="double"/>
+    </Decorator>
+    <Decorator ID="InPose">
+      <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="backwards"
+                  default="false"
+                  type="bool"/>
+      <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>
+    <Condition ID="SetInitialPose">
+      <input_port name="y"
+                  default="0.000000"
+                  type="double">Y position in meters</input_port>
+      <input_port name="theta"
+                  default="0.000000"
+                  type="double">Heading in degrees</input_port>
+      <input_port name="frame_id"
+                  default="map"
+                  type="std::string">Reference frame</input_port>
+      <input_port name="x"
+                  default="0.000000"
+                  type="double">X position in meters</input_port>
+      <input_port name="topic_name"
+                  default="__default__placeholder__"
+                  type="std::string">Topic name</input_port>
+    </Condition>
+    <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="WaitPullPin">
+      <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>

toid_bt/behavior_trees/calibration_routines.xml

@@ -1,45 +1,144 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
-  <BehaviorTree ID="wheel_ratio_calibration">
+  <BehaviorTree ID="track_calib">
     <Sequence>
-      <Delay delay_msec="5000">
+      <SetBlackboard value="0.265"
-        <RotateSimple angle="0"
+                     output_key="width"/>
+      <WaitPullPin />
+      <Sleep msec="1000"/>
+      <ZeroOdom service_name=""/>
+      <RotateTowards x="0.4"
+                     y="0.0"
                      max_speed="0.000000"
-                      min_angle="270"
+                     min_angle="0.000000"
                      action_name=""/>
-      </Delay>
       <Sequence>
-        <MovePointSimple x="1.0"
+        <Sleep msec="1000"/>
+        <MovePointSimple x="0.4"
                          y="0"
                          theta="0"
                          max_speed="0.10000"
+                         backwards="false"
                          action_name=""/>
-        <RotateSimple angle="180"
+        <Sleep msec="500"/>
-                      max_speed="0.500000"
-                      min_angle="10"
-                      action_name=""/>
-        <MovePointSimple x="0.4"
-                         y="0"
-                         theta="180"
-                         max_speed="0.100000"
-                         action_name=""/>
         <RotateSimple angle="0"
                       max_speed="0.500000"
-                      min_angle="-10"
+                      min_angle="700"
                       action_name=""/>
       </Sequence>
-      <TranslateX x="-0.5"
+      <ForceSuccess>
-                  max_speed="0.050000"
+        <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="2"
+                new_width="{width}"
+                service_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="wheel_ratio_calibration">
+    <Sequence>
+      <Sleep msec="5000"/>
+      <ZeroOdom service_name=""/>
+      <WaitPullPin />
+      <Sleep msec="1000"/>
+      <Repeat num_cycles="5">
+        <Sequence>
+          <MovePointSimple x="1.1"
+                           y="0"
+                           theta="0"
+                           max_speed="0.30000"
+                           backwards="false"
+                           action_name=""/>
+          <Sleep msec="1000"/>
+          <RotateSimple angle="180"
+                        max_speed="1.300000"
+                        min_angle="10"
+                        action_name=""/>
+          <Sleep msec="500"/>
+          <MovePointSimple x="0.35"
+                           y="0"
+                           theta="180"
+                           max_speed="0.300000"
+                           backwards="false"
                            action_name=""/>
           <RotateSimple angle="0"
-                    max_speed="0.000000"
+                        max_speed="1.300000"
-                    min_angle="-270"
+                        min_angle="-10"
+                        action_name=""/>
+          <Sleep msec="500"/>
+          <MovePointSimple x="1.1"
+                           y="0"
+                           theta="0"
+                           max_speed="0.30000"
+                           backwards="false"
+                           action_name=""/>
+          <Sleep msec="500"/>
+          <RotateSimple angle="180"
+                        max_speed="1.300000"
+                        min_angle="-10"
+                        action_name=""/>
+          <Sleep msec="500"/>
+          <MovePointSimple x="0.35"
+                           y="0"
+                           theta="180"
+                           max_speed="0.300000"
+                           backwards="false"
+                           action_name=""/>
+          <RotateSimple angle="0"
+                        max_speed="1.300000"
+                        min_angle="10"
+                        action_name=""/>
+        </Sequence>
+      </Repeat>
+      <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>
+      <WaitPullPin />
+      <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=""/>
     </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"/>
@@ -50,6 +149,9 @@
       <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>
@@ -65,15 +167,35 @@
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
     </Action>
-    <Action ID="TranslateX">
+    <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="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>

toid_bt/behavior_trees/toid_behaviors.btproj

@@ -4,14 +4,35 @@
     <include path="calibration_routines.xml"/>
     <!-- Description of Node Models (used by Groot) -->
     <TreeNodesModel>
+        <Action ID="ApproachAcorns">
+            <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>
+        <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>
+        <Decorator ID="InPose">
+            <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="RotateAcorns">
+            <input_port name="angle" 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="RotateSimple">
@@ -23,10 +44,33 @@
         <Action ID="RotateTowards">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
+            <input_port name="backwards" default="false" type="bool"/>
             <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>
+        <Condition ID="SetInitialPose">
+            <input_port name="y" default="0.000000" type="double">Y position in meters</input_port>
+            <input_port name="theta" default="0.000000" type="double">Heading in degrees</input_port>
+            <input_port name="frame_id" default="map" type="std::string">Reference frame</input_port>
+            <input_port name="x" default="0.000000" type="double">X position in meters</input_port>
+            <input_port name="topic_name" default="__default__placeholder__" type="std::string">Topic name</input_port>
+        </Condition>
+        <Action ID="SetParameter">
+            <input_port name="parameter" type="std::string"/>
+            <input_port name="value" type="std::string"/>
+            <input_port name="node" type="std::string"/>
+        </Action>
         <Action ID="SetWidth">
             <input_port name="width" type="double"/>
             <input_port name="count" default="1" type="int"/>
@@ -38,6 +82,9 @@
             <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="WaitPullPin">
+            <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>

toid_bt/include/toid_bt/bt_executor.hpp

@@ -8,6 +8,7 @@
 #include "tf2_ros/transform_listener.hpp"
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "std_msgs/msg/string.hpp"
 
 namespace toid {
     class TreeExecutor : public BT::TreeExecutionServer {
@@ -21,6 +22,12 @@ namespace toid {
 
         void position(geometry_msgs::msg::PoseStamped &pose);
 
+        void acorn_position(geometry_msgs::msg::PoseStamped &msg);
+
+        void acorn_pose_cb(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
+
+        void to_flip_cb(std_msgs::msg::String::ConstSharedPtr msg);
+
         std::string describeCustomNodes();
 
       private:
@@ -28,6 +35,12 @@ namespace toid {
         tf2_ros::Buffer::SharedPtr                  tf_buffer_;
         std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
+        rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr acorn_pose_sub_;
+        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr on_rotate_sub_;
+
+        geometry_msgs::msg::PoseStamped acorn_pose_;
+        std::string to_flip_ = "0000";
+
         std::string base_frame_;
         std::string global_frame_;
     };

toid_bt/include/toid_bt/plugin.hpp

@@ -5,4 +5,5 @@
 
 namespace toid {
     using PoseFunc = std::function<void(geometry_msgs::msg::PoseStamped &)>;
+    using FlipFunc = std::function<void(std::string &)>;
 }

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();
-    auto count = getInput<double>("count").value();
+    double count = getInput<int>("count").value();
 
     geometry_msgs::msg::PoseStamped pose;
     get_pose(pose);
@@ -42,6 +42,8 @@ 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;
   }

toid_bt/include/toid_bt/plugins/in_position_decorator.hpp

@@ -0,0 +1,74 @@
+#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 InPositionNode : public BT::DecoratorNode
+{
+public:
+  InPositionNode(
+    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 = abs(poseA.pose.position.x - poseB.pose.position.x);
+        return dx < 0.005;
+    }
+
+    bool checkIfInPose(geometry_msgs::msg::PoseStamped &pose) {
+        const double dx = abs(0.2575 - pose.pose.position.x);
+        return dx < 0.005;
+    }
+
+    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(checkIfInPose(pose)) {
+            if(checkIfPosesAreClose(last_pos, pose)) {
+                if(rclcpp::Time(pose.header.stamp) - last_pos_change > rclcpp::Duration::from_seconds(timeout)) {
+                    haltChild();
+                    return BT::NodeStatus::SUCCESS;
+                }
+            } else {
+                last_pos = pose;
+                last_pos_change = pose.header.stamp;
+            }
+        }
+
+        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

toid_bt/include/toid_bt/plugins/move_coords_action.hpp

@@ -28,6 +28,7 @@ public:
       BT::InputPort<double>("y"),
       BT::InputPort<double>("theta"),
       BT::InputPort<double>("max_speed", 0, {}),
+      BT::InputPort<bool>("backwards", false, {}),
       //BT::InputPort<double>("options"),
     });
   }
@@ -38,6 +39,7 @@ 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();
@@ -50,6 +52,8 @@ public:
     }
 
     goal.max_speed = max_speed;
+    goal.backwards = backwards;
+    goal.mode = 1;
 
     return true;
   }

toid_bt/include/toid_bt/plugins/pull_pin_action.hpp

@@ -0,0 +1,48 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+#include "toid_msgs/action/empty_action.hpp"
+
+namespace toid
+{
+
+class PullPinNode : public BT::RosActionNode<toid_msgs::action::EmptyAction>
+{
+public:
+  PullPinNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosActionNode<toid_msgs::action::EmptyAction>(name, conf, params)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      //BT::InputPort<double>("options"),
+    });
+  }
+
+  bool setGoal(Goal &) override
+  {
+    return true;
+  }
+
+  void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
+
+  BT::NodeStatus onResultReceived(const WrappedResult &) override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
+  {
+    RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+    return BT::NodeStatus::FAILURE;
+  }
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/rotate_action.hpp

@@ -40,6 +40,7 @@ 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;
   }
 

toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp

@@ -25,6 +25,7 @@ public:
     return providedBasicPorts({
       BT::InputPort<double>("x", {}), 
       BT::InputPort<double>("y", {}), 
+      BT::InputPort<bool>("backwards", false, {}),
       BT::InputPort<double>("min_angle", 0, {}),
       BT::InputPort<double>("max_speed", 0, {}),
       //BT::InputPort<double>("options"),
@@ -37,13 +38,20 @@ public:
     auto y = getInput<double>("y").value();
     auto min_angle = getInput<double>("min_angle").value();
     auto max_speed = getInput<double>("max_speed").value();
+    auto backwards = getInput<bool>("backwards").value();
 
     geometry_msgs::msg::PoseStamped pose;
     get_pose(pose);
 
+    if(!backwards) {
       goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x);
+    }
+    if(backwards) {
+      goal.angle = std::atan2(pose.pose.position.y - y, pose.pose.position.x - x);
+    }
     goal.min_angle = angles::from_degrees(min_angle);
     goal.max_speed = max_speed;
+    goal.mode = 1;
 
     return true;
   }

toid_bt/include/toid_bt/plugins/send_text_action.hpp

@@ -0,0 +1,48 @@
+#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, FlipFunc get_text)
+  : BT::RosServiceNode<toid_msgs::srv::SendString>(name, conf, params), get_text(get_text)
+  {
+  }
+
+  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("");
+    if(text=="") {
+      get_text(text);
+    }
+    req->text = text;
+
+    return true;
+  }
+
+  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+protected:
+  FlipFunc get_text;
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp

@@ -0,0 +1,60 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_topic_pub_node.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+class SetInitialPoseNode
+: public BT::RosTopicPubNode<geometry_msgs::msg::PoseWithCovarianceStamped>
+{
+public:
+  SetInitialPoseNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosTopicPubNode<geometry_msgs::msg::PoseWithCovarianceStamped>(name, conf, params)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<double>("x", 0.0, "X position in meters"),
+      BT::InputPort<double>("y", 0.0, "Y position in meters"),
+      BT::InputPort<double>("theta", 0.0, "Heading in degrees"),
+      BT::InputPort<std::string>("frame_id", "map", "Reference frame"),
+    });
+  }
+
+  bool setMessage(geometry_msgs::msg::PoseWithCovarianceStamped & msg) override
+  {
+    double x = getInput<double>("x").value_or(0.0);
+    double y = getInput<double>("y").value_or(0.0);
+    double theta_deg = getInput<double>("theta").value_or(0.0);
+    std::string frame_id = getInput<std::string>("frame_id").value_or("map");
+
+    msg.header.stamp = node_->get_clock()->now();
+    msg.header.frame_id = frame_id;
+
+    msg.pose.pose.position.x = x;
+    msg.pose.pose.position.y = y;
+    msg.pose.pose.position.z = 0.0;
+
+    tf2::Quaternion q;
+    q.setRPY(0.0, 0.0, angles::from_degrees(theta_deg));
+    msg.pose.pose.orientation = tf2::toMsg(q);
+
+    // Default covariance: small diagonal for x, y, yaw
+    msg.pose.covariance = {};
+    msg.pose.covariance[0] = 0.25;   // x
+    msg.pose.covariance[7] = 0.25;   // y
+    msg.pose.covariance[35] = 0.068; // yaw
+
+    return true;
+  }
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/set_parameter_action.hpp

@@ -0,0 +1,50 @@
+#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"
+#include "rcl_interfaces/srv/set_parameters.hpp"
+
+namespace toid
+{
+
+class SetParameterNode : public BT::RosServiceNode<rcl_interfaces::srv::SetParameters>
+{
+public:
+  SetParameterNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosServiceNode<rcl_interfaces::srv::SetParameters>(name, conf, params)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<std::string>("node"),
+      BT::InputPort<std::string>("parameter"),
+      BT::InputPort<std::string>("value")
+    });
+  }
+
+  bool setRequest(typename Request::SharedPtr & request) override { 
+    std::string parameter = getInput<std::string>("parameter").value();
+    std::string value = getInput<std::string>("value").value();
+    std::string node = getInput<std::string>("node").value();
+
+    setServiceName("/" + node + "/set_parameters");
+    request->parameters.emplace_back();
+    request->parameters.front().name = parameter;
+    request->parameters.front().value.string_value = value;
+    return true; 
+}
+
+  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+};
+
+}  // namespace

toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp

@@ -0,0 +1,69 @@
+#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

toid_bt/include/toid_bt/plugins/translate_x_action.hpp

@@ -5,18 +5,18 @@
 #include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "toid_bt/plugin.hpp"
-#include "toid_msgs/action/simple_translate_x.hpp"
+#include "toid_msgs/action/simple_move_coords.hpp"
 
 namespace toid
 {
 
-class TranslateXNode : public BT::RosActionNode<toid_msgs::action::SimpleTranslateX>
+class TranslateXNode : public BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>
 {
 public:
   TranslateXNode(
     const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
     PoseFunc pose)
-  : BT::RosActionNode<toid_msgs::action::SimpleTranslateX>(name, conf, params), get_pose(pose)
+  : BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>(name, conf, params), get_pose(pose)
   {
   }
 
@@ -34,7 +34,16 @@ public:
     auto x_goal = getInput<double>("x").value();
     auto max_speed = getInput<double>("max_speed").value();
 
-    goal.distance = x_goal;
+    geometry_msgs::msg::PoseStamped pose;
+    get_pose(pose);
+
+    double yaw = tf2::getYaw(pose.pose.orientation);
+
+    goal.x = pose.pose.position.x + cos(yaw) * x_goal;
+    goal.y = pose.pose.position.y + sin(yaw) * x_goal;
+    goal.theta = yaw;
+    goal.backwards = x_goal < 0;
+    goal.mode = 1;
     goal.max_speed = max_speed;
 
     return true;

toid_bt/launch/bt.launch.py

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

toid_bt/package.xml

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

toid_bt/src/bt_executor.cpp

@@ -6,9 +6,15 @@
 #include "rclcpp/rclcpp.hpp"
 #include "toid_bt/plugins/empty_srv_action.hpp"
 #include "toid_bt/plugins/end_calib_action.hpp"
+#include "toid_bt/plugins/in_position_decorator.hpp"
 #include "toid_bt/plugins/move_coords_action.hpp"
+#include "toid_bt/plugins/pull_pin_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/set_parameter_action.hpp"
+#include "toid_bt/plugins/stuck_detector_decorator.hpp"
+#include "toid_bt/plugins/set_initial_pose_action.hpp"
 #include "toid_bt/plugins/translate_x_action.hpp"
 
 namespace toid
@@ -19,9 +25,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"));
@@ -30,6 +36,13 @@ TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts)
   nav2_util::declare_parameter_if_not_declared(
     node(), "global_frame", rclcpp::ParameterValue("map"));
   node()->get_parameter("global_frame", global_frame_);
+  using namespace std::placeholders;
+
+  acorn_pose_sub_ = node()->create_subscription<geometry_msgs::msg::PoseStamped>(
+    "/closest_acorn", rclcpp::QoS(1), std::bind(&TreeExecutor::acorn_pose_cb, this, _1));
+
+  on_rotate_sub_ = node()->create_subscription<std_msgs::msg::String>(
+    "/to_flip", rclcpp::QoS(1), std::bind(&TreeExecutor::to_flip_cb, this, _1));
 }
 
 void TreeExecutor::onTreeCreated(BT::Tree & tree)
@@ -39,27 +52,62 @@ void TreeExecutor::onTreeCreated(BT::Tree & tree)
 
 void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
 {
+  FlipFunc get_to_flip = [this](std::string & pose) { pose = this->to_flip_; };
   PoseFunc get_pose = [this](geometry_msgs::msg::PoseStamped & pose) { this->position(pose); };
+  PoseFunc get_acorn_pose = [this](geometry_msgs::msg::PoseStamped & pose) {
+    this->acorn_position(pose);
+  };
+
   rclcpp::Node::SharedPtr nh = node();
   factory.registerNodeType<toid::MovePointNode>(
     "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
 
+  factory.registerNodeType<toid::MovePointNode>(
+    "ApproachAcorns", BT::RosNodeParams(nh, "/approachAcorns"), get_pose);
+
   factory.registerNodeType<toid::RotateNode>(
     "RotateSimple", BT::RosNodeParams(nh, "/rotate"), get_pose);
 
+  factory.registerNodeType<toid::RotateNode>(
+    "RotateAcorns", BT::RosNodeParams(nh, "/rotateAcorns"), get_pose);
+
   factory.registerNodeType<toid::RotateTowardsNode>(
     "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose);
 
   factory.registerNodeType<toid::TranslateXNode>(
-    "TranslateX", BT::RosNodeParams(nh, "/translateX"), get_pose);
+    "TranslateX", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
 
   factory.registerNodeType<toid::EndCalibNode>(
     "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose);
 
+  factory.registerNodeType<toid::PullPinNode>("WaitPullPin", BT::RosNodeParams(nh, "/start_plug"));
+
+  factory.registerNodeType<toid::SetParameterNode>(
+    "SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters"));
+
+  factory.registerNodeType<toid::SetInitialPoseNode>(
+    "SetInitialPose", BT::RosNodeParams(nh, "/initialpose"));
+
   factory.registerNodeType<toid::EmptySrvNode>("ZeroOdom", BT::RosNodeParams(nh, "/zero"));
 
   factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
 
+  BT::RosNodeParams service_params1(nh, "/sequence1");
+  service_params1.server_timeout = std::chrono::seconds(15);
+  factory.registerNodeType<toid::EmptySrvNode>("Seq1", service_params1);
+
+  BT::RosNodeParams service_params2(nh, "/sequence2");
+  service_params2.server_timeout = std::chrono::seconds(20);
+  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params2, get_to_flip);
+
+  BT::RosNodeParams service_params3(nh, "/sequence3");
+  service_params3.server_timeout = std::chrono::seconds(15);
+  factory.registerNodeType<toid::EmptySrvNode>("Seq3", service_params3);
+
+  factory.registerNodeType<toid::StuckDetectorNode>("DetectStuck", get_pose);
+
+  factory.registerNodeType<toid::InPositionNode>("InPose", get_acorn_pose);
+
   std::cout << describeCustomNodes() << std::endl;
 }
 
@@ -68,6 +116,23 @@ void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose)
   nav2_util::getCurrentPose(pose, *tf_buffer_, global_frame_, base_frame_);
 }
 
+void TreeExecutor::acorn_position(geometry_msgs::msg::PoseStamped & pose) { pose = acorn_pose_; }
+
+void TreeExecutor::acorn_pose_cb(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
+{
+  auto node = this->node();
+  try {
+    acorn_pose_ = tf_buffer_->transform(*msg, "base_footprint");
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_ERROR_THROTTLE(node->get_logger(), *node->get_clock(), 5000, "Transform timeout");
+  }
+}
+
+void TreeExecutor::to_flip_cb(std_msgs::msg::String::ConstSharedPtr msg)
+{
+  this->to_flip_ = msg->data;
+}
+
 std::optional<std::string> TreeExecutor::onTreeExecutionCompleted(
   BT::NodeStatus status, bool was_cancelled)
 {

toid_cli/setup.cfg.old

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

toid_cli/toid_cli/main.py

@@ -1,70 +0,0 @@
-from fastapi import FastAPI
-from pydantic import BaseModel
-from contextlib import asynccontextmanager
-from fastapi.middleware.cors import CORSMiddleware
-import asyncio
-import rclpy
-import uvicorn
-import threading
-import logging
-
-import toid_cli.routers.startup as startup
-import toid_cli.routers.action as action
-from toid_cli.services.services import actionClient, ServerRunner, log
-from toid_cli.services.runners import main_runner
-
-logging.getLogger().setLevel(logging.INFO)
-
-@asynccontextmanager
-async def lifespan(app: FastAPI):
-    rclpy.init()
-    ServerRunner.setRunner(ServerRunner(app))
-    thread = threading.Thread(target=rclpy.spin, args=(ServerRunner.getRunner(),), daemon=True)
-    thread.start()
-    log.info("Started up rclpy")
-    if not hasattr(app.state, 'loop'):
-        app.state.loop = asyncio.get_running_loop()
-
-    yield
-
-    await main_runner.stop_robot()
-    log.info("Stopped robot")
-    ServerRunner.getRunner().destroy_node()
-    rclpy.shutdown()
-    thread.join()
-    log.info("Closed rclpy")
-
-
-
-app = FastAPI(lifespan=lifespan)
-app.add_middleware(
-    CORSMiddleware,
-    allow_origins=[
-        "*"
-    ],
-    allow_credentials=True,
-    allow_methods=["*"],
-    allow_headers=["*"],
-)
-
-
-
-app.include_router(startup.router, prefix="/startup")
-app.include_router(action.router, prefix="/action")
-# store running launch processes
-launch_processes = {}
-
-
-
-
-def main():
-    uvicorn.run(
-        "toid_cli.main:app",
-        host="0.0.0.0",
-        port=8000,
-        reload=False
-    )
-
-
-if __name__ == "__main__":
-    main()

toid_cli/toid_cli/routers/action.py

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

toid_cli/toid_cli/routers/startup.py

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

toid_cli/toid_cli/services/runners.py

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

toid_cli/toid_cli/services/services.py

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

toid_control/launch/toid.launch.py

@@ -6,92 +6,101 @@ from launch_ros.actions import Node, LifecycleNode
 from launch_ros.substitutions import FindPackageShare
 import os
 
+
 def generate_launch_description():
 
-    pkg_share = FindPackageShare("").find('toid_control')
+    pkg_share = FindPackageShare("").find("toid_control")
-    params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
+    params = os.path.join(pkg_share, "params", "toid_general_params.yaml")
-    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'rviz.rviz')
+    default_rviz_config_path = os.path.join(pkg_share, "rviz", "rviz.rviz")
 
-    description_pkg_share = FindPackageShare("").find('toid_bot_description')
+    description_pkg_share = FindPackageShare("").find("toid_bot_description")
     default_model_path = os.path.join(
-        description_pkg_share, 
+        description_pkg_share, "src", "toid_bot_description.urdf"
-        'src', 
-        'toid_bot_description.urdf'
     )
 
     visualize = LaunchConfiguration("visualize")
 
     visualize_arg = DeclareLaunchArgument(
-        'visualize',
+        "visualize", default_value="False", description="Whether to launch rviz2"
-        default_value='False',
-        description="Whether to launch rviz2" 
     )
 
     use_mock = LaunchConfiguration("use_mock")
 
     use_mock_arg = DeclareLaunchArgument(
-        'use_mock',
+        "use_mock", default_value="True", description="Whether to use mock controller"
-        default_value='True',
-        description="Whether to use mock controller" 
     )
 
     odom_broadcast = Node(
-        package='toid_odometry',
+        package="toid_odometry",
-        executable='toid_odometry',
+        executable="toid_odometry",
-        name='map_to_odom_broadcaster',
+        name="map_to_odom_broadcaster",
         emulate_tty=True,
-        parameters=[{'mock_odom': use_mock}],
+        parameters=[
+            {
+                "mock_odom": use_mock,
+                "serial_port": "/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if02",
+            }
+        ],
     )
 
-
     robot_state_publisher = Node(
-        package='robot_state_publisher',
+        package="robot_state_publisher",
-        executable='robot_state_publisher',
+        executable="robot_state_publisher",
-        name='robot_state_publisher',
+        name="robot_state_publisher",
-        output='screen',
+        output="screen",
         emulate_tty=True,
-        parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}]
+        parameters=[
+            {
+                "robot_description": Command(
+                    [
+                        "xacro ",
+                        default_model_path,
+                        " use_mock:=",
+                        use_mock,
+                        " serial_port:=/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if00",
+                    ]
+                )
+            }
+        ],
     )
 
     controller_manager = Node(
-        package='controller_manager',
+        package="controller_manager",
-        executable='ros2_control_node',
+        executable="ros2_control_node",
-        output='screen',
+        output="screen",
         emulate_tty=True,
         parameters=[params],
-        arguments=['--ros-args', '--log-level', 'warn']
+        arguments=["--ros-args", "--log-level", "warn"],
     )
 
     joint_state_broadcaster = Node(
-        package='controller_manager',
+        package="controller_manager",
-        executable='spawner',
+        executable="spawner",
-        output='screen',
+        output="screen",
         emulate_tty=True,
-        arguments=[
+        arguments=["joint_state_broadcaster", "--ros-args", "--log-level", "warn"],
-            "joint_state_broadcaster",
-            '--ros-args', '--log-level', 'warn'
-            ]
     )
 
     velocity_controller = Node(
-        package='controller_manager',
+        package="controller_manager",
-        executable='spawner',
+        executable="spawner",
-        output='screen',
+        output="screen",
         emulate_tty=True,
         arguments=[
             "velocity_controller",
             "--inactive",
             "-p",
             params,
-            '--ros-args', '--log-level', 'warn'
+            "--ros-args",
+            "--log-level",
+            "warn",
         ],
     )
 
-
     diffbot_base_controller = Node(
-        package='controller_manager',
+        package="controller_manager",
-        executable='spawner',
+        executable="spawner",
-        output='both',
+        output="both",
         emulate_tty=True,
         arguments=[
             "diffdrive_controller",
@@ -102,32 +111,35 @@ def generate_launch_description():
             "--controller-ros-args",
             "-r diffdrive_controller/odom:=/odom",
             "--controller-ros-args",
-            IfElseSubstitution(use_mock, 
+            IfElseSubstitution(
+                use_mock,
                 "--param odom_frame_id:=odom",
-                "--param odom_frame_id:=odom_expected"
+                "--param odom_frame_id:=odom_expected",
             ),
             "--controller-ros-args",
-            IfElseSubstitution(use_mock, 
+            IfElseSubstitution(
+                use_mock,
                 "--param enable_odom_tf:=true",
-                "--param enable_odom_tf:=false"
+                "--param enable_odom_tf:=false",
             ),
-            '--ros-args', '--log-level', 'warn'
+            "--ros-args",
-        ]
+            "--log-level",
+            "warn",
+        ],
     )
 
     rviz_node = Node(
-        package='rviz2',
+        package="rviz2",
-        executable='rviz2',
+        executable="rviz2",
-        name='rviz2',
+        name="rviz2",
-        output='screen',
+        output="screen",
         emulate_tty=True,
-        arguments=['-d', default_rviz_config_path, 
+        arguments=["-d", default_rviz_config_path, "--ros-args", "--log-level", "warn"],
-                   '--ros-args', '--log-level', 'warn'
+        condition=IfCondition(visualize),
-                   ],
-        condition=IfCondition(visualize)
     )
 
-    return LaunchDescription([
+    return LaunchDescription(
+        [
             visualize_arg,
             use_mock_arg,
             odom_broadcast,
@@ -136,5 +148,6 @@ def generate_launch_description():
             joint_state_broadcaster,
             diffbot_base_controller,
             velocity_controller,
-        rviz_node
+            rviz_node,
-    ])
+        ]
+    )

toid_control/src/toid_control.cpp

@@ -82,6 +82,15 @@ hardware_interface::return_type toid::StepperInterface::write(const rclcpp::Time
         boost::system::error_code ec;
         ec = serial_port_.close(ec);
         ec = serial_port_.open(serial_port_name_, ec);
+        if(!ec.failed()) {
+            try {
+                for(int i = 0; i < 64; i++) {
+                    asio::write(serial_port_, asio::buffer("\x00"));
+                }
+            } catch(const std::runtime_error& err) {
+                
+            }
+        }
     }
     return hardware_interface::return_type::OK;
 }

toid_costmaps/CMakeLists.txt

@@ -0,0 +1,83 @@
+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()

toid_costmaps/LICENSE

@@ -0,0 +1,202 @@
+
+                                 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.

toid_costmaps/elements/elements.json

@@ -0,0 +1,46 @@
+{
+  "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
+    }
+  ]
+}

toid_costmaps/include/toid_costmaps/element_info.hpp

@@ -0,0 +1,46 @@
+#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

toid_costmaps/include/toid_costmaps/game_elements_layer.hpp

@@ -0,0 +1,58 @@
+#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

toid_costmaps/include/toid_costmaps/rival_layer.hpp

@@ -0,0 +1,55 @@
+#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

toid_costmaps/package.xml

@@ -0,0 +1,32 @@
+<?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>

toid_costmaps/src/game_elements_layer.cpp

@@ -0,0 +1,138 @@
+#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);

toid_costmaps/src/rival_layer.cpp

@@ -0,0 +1,108 @@
+
+#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);

toid_costmaps/toid_costmaps.xml

@@ -0,0 +1,10 @@
+<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>

toid_frontend/.editorconfig

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

toid_frontend/.gitattributes

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

toid_frontend/.gitignore

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

toid_frontend/.oxlintrc.json

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

toid_frontend/.prettierrc.json

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

toid_frontend/.vscode/extensions.json

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

toid_frontend/README.md

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

toid_frontend/env.d.ts

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

toid_frontend/eslint.config.ts

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

toid_frontend/index.html

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

toid_frontend/package.json

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

toid_frontend/src/App.vue

@@ -1,50 +0,0 @@
-<script setup lang="ts">
-import { computed } from 'vue'
-import { connectionStatus } from './stores/connection-status'
-import { ros } from './ts/ros_client'
-import { rosBackend } from './ts/robot_bridge'
-
-const store = connectionStatus()
-const connected = computed(() => {
-  return store.backendConnected && store.rosbridgeConnected
-})
-
-const reconnect = () => {
-  ros.manualConnect()
-  rosBackend.manualConnect()
-}
-
-const rosout = ros.ros.Topic({
-  name: '/rosout',
-  messageType: 'rcl_interfaces/msg/Log',
-})
-
-rosout.subscribe((msg) => {
-  console.log(msg)
-})
-</script>
-
-<template>
-  <div>
-    <div v-if="!connected" class="box-container">
-      Connecting...
-      <button id="reconnect" @click="reconnect">Reconnect</button>
-    </div>
-    <div class="box-container">
-      <h3>Current position:</h3>
-    </div>
-  </div>
-</template>
-
-<style scoped>
-.box-container {
-  margin: 30px;
-  padding: 10px;
-  border: 2px solid white;
-}
-
-#reconnect {
-  margin-left: 2rem;
-  background-color: green;
-}
-</style>

toid_frontend/src/assets/scss/global.scss

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

toid_frontend/src/components/PageBanner.vue

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

toid_frontend/src/components/RobotControl.vue

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

toid_frontend/src/components/RobotViewport.vue

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

toid_frontend/src/main.ts

@@ -1,13 +0,0 @@
-import { createApp } from 'vue'
-import { createPinia } from 'pinia'
-
-import App from './App.vue'
-import router from './router'
-import '@/assets/scss/global.scss'
-import '@/ts/robot_bridge'
-import '@/ts/ros_client'
-
-const app = createApp(App)
-app.use(createPinia())
-app.use(router)
-app.mount('#app')

toid_frontend/src/router/index.ts

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

toid_frontend/src/stores/connection-status.ts

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

toid_frontend/src/stores/counter.ts

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

toid_frontend/src/ts/robot_bridge.ts

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

toid_frontend/src/ts/ros_client.ts

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

toid_frontend/src/utils/smoothControlLaw.ts

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

toid_frontend/tsconfig.app.json

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

toid_frontend/tsconfig.json

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

toid_frontend/tsconfig.node.json

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

toid_frontend/vite.config.ts

@@ -1,18 +0,0 @@
-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))
-    },
-  },
-})

toid_interaction/package.xml

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

toid_interaction/setup.cfg

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