Added basic README

.gitignore

@@ -1,3 +1,4 @@
+__pycache__
 .cache
 build
 install

.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

@@ -8,11 +8,30 @@ services:
     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"]

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/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/simple_move.hpp

@@ -127,17 +127,20 @@ public:
 
   bool check_rival_collision(geometry_msgs::msg::Pose2D & pose)
   {
-    if (!rivals_) {
+    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 = 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;
@@ -148,6 +151,7 @@ public:
       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;
       }
@@ -166,7 +170,7 @@ protected:
 
   double robot_width_ = 0.30;
   double robot_length_ = 0.30;
-  double rival_radius_ = 0.30;
+  double rival_radius_ = 0.22;
 
   Rival::SharedPtr rivals_;
   rclcpp::Subscription<Rival>::SharedPtr rivals_sub_;

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/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/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,9 +1,16 @@
 <?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="0101"
+      <Seq2 text="1010"
             service_name=""/>
       <Seq3 service_name=""/>
     </Sequence>
@@ -12,21 +19,240 @@
   <BehaviorTree ID="seq1">
     <Sequence>
       <ZeroOdom service_name=""/>
-      <MovePointSimple x="1.05"
-                       y="0"
-                       theta="0"
+      <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=""/>
-      <MovePointSimple x="0.76"
-                       y="0.18"
+      <WaitPullPin action_name=""/>
+      <Timeout msec="100000">
+        <Sequence>
+          <InPose timeout="1.000000">
+            <MovePointSimple x="1.33"
+                             y="0.354"
                              theta="-90"
-                       max_speed="0.250000"
+                             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=""/>
-      <TranslateX x="0.5"
-                  max_speed="0.300000"
+          <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>
 
@@ -35,6 +261,7 @@
       <DetectStuck timeout="1.000000">
         <RotateTowards x="0.4"
                        y="0.0"
+                       backwards="false"
                        max_speed="0.000000"
                        min_angle="0.000000"
                        action_name=""/>
@@ -49,6 +276,11 @@
                   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"/>
@@ -70,6 +302,9 @@
                   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"/>
@@ -93,6 +328,23 @@
       <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"/>
@@ -102,6 +354,10 @@
       <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>

toid_bt/behavior_trees/calibration_routines.xml

@@ -4,6 +4,7 @@
     <Sequence>
       <SetBlackboard value="0.265"
                      output_key="width"/>
+      <WaitPullPin />
       <Sleep msec="1000"/>
       <ZeroOdom service_name=""/>
       <RotateTowards x="0.4"
@@ -22,7 +23,7 @@
         <Sleep msec="500"/>
         <RotateSimple angle="0"
                       max_speed="0.500000"
-                      min_angle="270"
+                      min_angle="700"
                       action_name=""/>
       </Sequence>
       <ForceSuccess>
@@ -36,7 +37,7 @@
         </DetectStuck>
       </ForceSuccess>
       <SetWidth width="{width}"
-                count="1"
+                count="2"
                 new_width="{width}"
                 service_name=""/>
     </Sequence>
@@ -46,31 +47,57 @@
     <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.10000"
+                           max_speed="0.30000"
                            backwards="false"
                            action_name=""/>
           <Sleep msec="1000"/>
           <RotateSimple angle="180"
-                      max_speed="0.300000"
+                        max_speed="1.300000"
                         min_angle="10"
                         action_name=""/>
           <Sleep msec="500"/>
           <MovePointSimple x="0.35"
                            y="0"
                            theta="180"
-                         max_speed="0.100000"
+                           max_speed="0.300000"
                            backwards="false"
                            action_name=""/>
           <RotateSimple angle="0"
-                      max_speed="0.300000"
+                        max_speed="1.300000"
                         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"
@@ -88,6 +115,7 @@
 
   <BehaviorTree ID="wheel_size">
     <Sequence>
+      <WaitPullPin />
       <Sleep msec="1000"/>
       <ZeroOdom service_name=""/>
       <Sleep msec="1000"/>

toid_bt/behavior_trees/toid_behaviors.btproj

@@ -4,12 +4,23 @@
     <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"/>
@@ -18,6 +29,12 @@
             <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">
             <input_port name="angle" type="double"/>
             <input_port name="max_speed" default="0.000000" type="double"/>
@@ -27,6 +44,7 @@
         <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>
@@ -41,6 +59,18 @@
         <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"/>
@@ -52,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/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/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_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,11 +38,17 @@ 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;

toid_bt/include/toid_bt/plugins/send_text_action.hpp

@@ -15,8 +15,8 @@ class SendTextNode : public BT::RosServiceNode<toid_msgs::srv::SendString>
 {
 public:
   SendTextNode(
-    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
-  : BT::RosServiceNode<toid_msgs::srv::SendString>(name, conf, params)
+    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)
   {
   }
 
@@ -29,6 +29,9 @@ public:
 
   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;
@@ -38,6 +41,8 @@ public:
   {
     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/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/src/bt_executor.cpp

@@ -6,11 +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
@@ -32,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)
@@ -41,37 +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"));
 
-  factory.registerNodeType<toid::EmptySrvNode>("Seq1", BT::RosNodeParams(nh, "/sequence1"));
+  BT::RosNodeParams service_params1(nh, "/sequence1");
+  service_params1.server_timeout = std::chrono::seconds(15);
+  factory.registerNodeType<toid::EmptySrvNode>("Seq1", service_params1);
 
-  BT::RosNodeParams service_params(nh, "/sequence2");
-  service_params.server_timeout = std::chrono::seconds(15);
-  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params);
+  BT::RosNodeParams service_params2(nh, "/sequence2");
+  service_params2.server_timeout = std::chrono::seconds(20);
+  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params2, get_to_flip);
 
-  factory.registerNodeType<toid::EmptySrvNode>("Seq3", BT::RosNodeParams(nh, "/sequence3"));
+  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;
 }
 
@@ -80,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_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')
-    params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
-    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'rviz.rviz')
+    pkg_share = FindPackageShare("").find("toid_control")
+    params = os.path.join(pkg_share, "params", "toid_general_params.yaml")
+    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, 
-        'src', 
-        'toid_bot_description.urdf'
+        description_pkg_share, "src", "toid_bot_description.urdf"
     )
 
     visualize = LaunchConfiguration("visualize")
 
     visualize_arg = DeclareLaunchArgument(
-        'visualize',
-        default_value='False',
-        description="Whether to launch rviz2" 
+        "visualize", default_value="False", description="Whether to launch rviz2"
     )
 
     use_mock = LaunchConfiguration("use_mock")
 
     use_mock_arg = DeclareLaunchArgument(
-        'use_mock',
-        default_value='True',
-        description="Whether to use mock controller" 
+        "use_mock", default_value="True", description="Whether to use mock controller"
     )
 
     odom_broadcast = Node(
-        package='toid_odometry',
-        executable='toid_odometry',
-        name='map_to_odom_broadcaster',
+        package="toid_odometry",
+        executable="toid_odometry",
+        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',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        output='screen',
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        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',
-        executable='ros2_control_node',
-        output='screen',
+        package="controller_manager",
+        executable="ros2_control_node",
+        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',
-        executable='spawner',
-        output='screen',
+        package="controller_manager",
+        executable="spawner",
+        output="screen",
         emulate_tty=True,
-        arguments=[
-            "joint_state_broadcaster",
-            '--ros-args', '--log-level', 'warn'
-            ]
+        arguments=["joint_state_broadcaster", "--ros-args", "--log-level", "warn"],
     )
 
     velocity_controller = Node(
-        package='controller_manager',
-        executable='spawner',
-        output='screen',
+        package="controller_manager",
+        executable="spawner",
+        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',
-        executable='spawner',
-        output='both',
+        package="controller_manager",
+        executable="spawner",
+        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',
-        executable='rviz2',
-        name='rviz2',
-        output='screen',
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="screen",
         emulate_tty=True,
-        arguments=['-d', default_rviz_config_path, 
-                   '--ros-args', '--log-level', 'warn'
-                   ],
-        condition=IfCondition(visualize)
+        arguments=["-d", default_rviz_config_path, "--ros-args", "--log-level", "warn"],
+        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_interaction/package.xml

@@ -13,6 +13,8 @@
   <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.py

@@ -25,7 +25,9 @@ setup(
     entry_points={
         'console_scripts': [
             'sequence = toid_interaction.mechanism.sekvenca_2026:main',
-            'node = toid_interaction.interaction_node:main'
+            'node = toid_interaction.interaction_node:main',
+            'cam_calib = toid_interaction.camera:main',
+            'cam_calib1 = toid_interaction.camera1:main'
         ],
     },
 )

toid_interaction/toid_interaction/interaction_node.py

@@ -1,97 +1,117 @@
 import rclpy
+import rclpy.callback_groups
+import rclpy.executors
 from rclpy.node import Node
 from std_srvs.srv import Empty
 
+from gpiozero import Button, OutputDevice
+
 from serial.tools import list_ports
 
 from toid_interaction.mechanism.sekvenca_2026 import okreni, okreni_niz
 from toid_interaction.mechanism.zidovi_load import ZidoviAction
+from toid_interaction.mechanism.zidovi import Zidovi
 from toid_interaction.mechanism.zupcanik import ZupcanikAction
+from toid_msgs.action import EmptyAction
 from toid_msgs.srv import SendString
 
+from rclpy.action.server import ServerGoalHandle
+from rclpy.action.server import ActionServer
+
+
 class InteracitionNode(Node):
     step: int = 0
+    btn_: Button
+    output_pin_: OutputDevice
+    start_pin_action_: ActionServer
+
     def __init__(self):
-        super().__init__('ToidInteractionNode')
+        super().__init__("ToidInteractionNode")
 
         self.find_sigma()
 
-        self.srv = self.create_service(
-            Empty,
-            '/sequence1',
-            self.sequence1_cb
-        )
-
-        self.srv = self.create_service(
-            SendString,
-            '/sequence2',
-            self.sequence2_cb
-        )
-
-        self.srv = self.create_service(
-            Empty,
-            '/sequence3',
-            self.sequence3_cb
-        )
-
-
-
+        self.srv = self.create_service(Empty, "/sequence1", self.sequence1_cb)
         self.get_logger().info("Service 'sequence1' ready.")
 
+        self.srv = self.create_service(SendString, "/sequence2", self.sequence2_cb)
+        self.get_logger().info("Service 'sequence2' ready.")
+
+        self.srv = self.create_service(Empty, "/sequence3", self.sequence3_cb)
+        self.get_logger().info("Service 'sequence3' ready.")
+
+        self.btn_ = Button(17)
+        self.output_pin_ = OutputDevice(27)
+
+        self.start_pin_action_ = ActionServer(
+            self, EmptyAction, "/start_plug", execute_callback=self.start_plug_action_cb,
+            callback_group=rclpy.callback_groups.ReentrantCallbackGroup()
+        )
+
+        self.get_logger().info("Action 'start_plug' ready.")
+
+
     def find_sigma(self):
         for port_info in list_ports.comports():
-            if port_info.vid == 0x18a6 and port_info.pid == 0x55d3:
+            if port_info.vid == 0x1A86 and port_info.pid == 0x55D3:
                 break
 
         print(port_info.device)
         self.st_motor_device_name = port_info.device
 
     def sequence1_cb(self, request, response):
-        if(self.step != 0):
+        if self.step != 0:
             return Empty.Response()
         okreni(5)
         zupcanik = ZupcanikAction(self.st_motor_device_name)
-        zupcanik.zupcanik(1, -1010, 25)
+        zupcanik.zupcanik(1, -1015, 25)
         self.step = 1
         return response
 
     def sequence2_cb(self, request: SendString.Request, response: SendString.Response):
-        if(self.step != 1):
-            return Empty.Response()
-        zidovi = ZidoviAction(self.st_motor_device_name)
-        zidovi.beli_zid(1)
-        zidovi.plavi_zid(1)
+        if self.step != 1:
+            return SendString.Response()
+
+        zidovi = Zidovi(self.st_motor_device_name)
+
+        zidovi.zidovi(1, 1500, 600, 210, 120, 120)
 
         okreni_niz(request.text)
 
-        zidovi.plavi_zid(0, TargetPos=150)
-        zidovi.beli_zid(0, TargetPos=450)
+        zidovi.zidovi(0, 1500, 420, 50, 120, 120)
         self.step = 2
         return response
 
     def sequence3_cb(self, request, response):
-        if(self.step != 2):
+        if self.step != 2:
             return Empty.Response()
 
         zupcanik = ZupcanikAction(self.st_motor_device_name)
-        zidovi = ZidoviAction(self.st_motor_device_name) 
+        zidovi = Zidovi(self.st_motor_device_name)
 
-        zupcanik.zupcanik(1, 1010, 25)
-        zidovi.plavi_zid(0, TargetPos=150)
-        zidovi.beli_zid(0, TargetPos=150)
+        zupcanik.zupcanik(1, 1015, 25)
+        zidovi.zidovi(0, 1500, 180, 160, 120, 120)
         okreni(5)
         self.step = 0
         return response
 
+    async def start_plug_action_cb(self, goal_handle: ServerGoalHandle):
+        while not self.btn_.is_active:
+            pass
+        while self.btn_.is_active:
+            pass
+        goal_handle.succeed()
+        return EmptyAction.Result()
+
 
 def main(args=None):
     rclpy.init(args=args)
 
     node = InteracitionNode()
-    rclpy.spin(node)
-
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(node)
+    executor.spin()
     rclpy.shutdown()
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     main()

toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py

@@ -76,6 +76,10 @@ class sts(protocol_packet_handler):
          sts_present_load, sts_comm_result,sts_error = self.read2ByteTxRx(sts_id, STS_PRESENT_LOAD_L)  # 60
          return self.sts_tohost(sts_present_load, 15), sts_comm_result, sts_error
     
+    def ReadCurrent(self, sts_id):
+         sts_present_current, sts_comm_result,sts_error = self.read2ByteTxRx(sts_id, STS_PRESENT_CURRENT_L)  # 69
+         return self.sts_tohost(sts_present_current, 15), sts_comm_result, sts_error
+
     def ReadMaxLoad(self, sts_id):
          sts_max_load, sts_comm_result,sts_error = self.read2ByteTxRx(sts_id, STS_LOAD_LIMIT)  # 0x36
          return sts_max_load, sts_comm_result, sts_error

toid_interaction/toid_interaction/mechanism/motori.py

@@ -0,0 +1,83 @@
+from .STservo_sdk.stservo_def import COMM_SUCCESS
+from .STservo_sdk import sts
+
+STS_MOVING_ACC = 0
+
+def get_load(packetHandler : sts, id_motora: int) -> float:
+    sts_present_load, _, _ = packetHandler.ReadLoad(id_motora)
+    return (sts_present_load & ((1 << 10) - 1)) * 0.1
+
+def get_current(packetHandler : sts, id_motora: int) -> float:
+    sts_present_current, _, _ = packetHandler.ReadCurrent(id_motora)
+    return (sts_present_current  * 6.5)
+
+def stop_motor(packetHandler : sts, id_motora: int):
+    packetHandler.WriteSpec(id_motora, 0, 0)
+
+def print_motor_error(packetHandler : sts, sts_com_result, sts_error, id_motora):
+    if sts_com_result != COMM_SUCCESS:
+        print("[ID:%d] %s" % (id_motora, packetHandler.getTxRxResult(sts_com_result)))
+    if sts_error != 0:
+        print("[ID:%d] %s" % (id_motora, packetHandler.getRxPacketError(sts_error)))
+
+def get_motor_pos_speed(packetHandler : sts, id_motora: int) -> tuple[float, float]:
+    sts_present_position, sts_present_speed, sts_com_result, sts_error = packetHandler.ReadPosSpeed(id_motora)
+    print_motor_error(packetHandler, sts_com_result, sts_error, id_motora)
+    
+    return sts_present_position, sts_present_speed
+
+def set_motor_speed(packetHandler : sts, id_motora: int, speed: float) -> tuple[float, float]:
+    sts_com_result, sts_error = packetHandler.WriteSpec(id_motora, speed, STS_MOVING_ACC)
+    print_motor_error(packetHandler, sts_com_result, sts_error, id_motora)
+
+def normalize_dpos(curr: float, prev: float) -> float:
+    if abs(curr - prev) > 3000:
+        return 4096 - abs(curr - prev)
+    else:
+        return abs(curr - prev)
+    
+class CrniMotor:
+    def __init__(self, packetHandler : sts, id_motora: int):
+        self.packetHandler = packetHandler
+        self.id_motora = id_motora
+        self.targret_reached = False
+        self.brzina = 0
+    
+
+        sts_comm_result, sts_error = packetHandler.WheelMode(id_motora)
+        if sts_comm_result != COMM_SUCCESS:
+            print("[ID:%01d] %s" % (id_motora, packetHandler.getTxRxResult(sts_comm_result)))
+        elif sts_error != 0:
+            print("[ID:%01d] %s" % (id_motora, packetHandler.getRxPacketError(sts_error)))
+
+        sts_present_position, sts_present_speed = get_motor_pos_speed(packetHandler, id_motora)
+        self.trenutna_pos = sts_present_position
+        self.prosla_pos = sts_present_position
+        self.predjeni_put = 0
+
+        packetHandler.SetMaxLoad(id_motora, 80)
+
+
+    def doCycle(self, target_pos: float, brzina: float) -> bool | None:
+        if not self.targret_reached:
+            sts_present_position, sts_present_speed = get_motor_pos_speed(self.packetHandler, self.id_motora)
+            self.trenutna_pos = sts_present_position
+            self.predjeni_put += normalize_dpos(self.trenutna_pos, self.prosla_pos)
+            self.prosla_pos = self.trenutna_pos
+
+            if self.predjeni_put >= target_pos:
+                self.stop()
+                self.targret_reached = True
+            else:
+                set_motor_speed(self.packetHandler, self.id_motora, brzina)
+                self.brzina = brzina
+
+            # TODO: if overload stop motor and return false:
+
+            return True
+        return True            
+    
+    def stop(self):
+        stop_motor(self.packetHandler, self.id_motora)
+        self.brzina = 0
+    

toid_interaction/toid_interaction/mechanism/sekvenca_2026.py

@@ -8,7 +8,7 @@ import time
 import serial
 import serial.tools.list_ports as list_ports
 
-SERIAL_ID = "50443405C8C3B21C"
+SERIAL_ID = "259B221729115453"
 
 
 def okreni(i):

toid_interaction/toid_interaction/mechanism/zidovi.py

@@ -0,0 +1,167 @@
+from .STservo_sdk import PortHandler, sts
+from .motori import get_load, stop_motor, get_current, CrniMotor
+import time
+
+
+BAUDRATE = 1000000
+STS_MOVING_ACC = 0
+
+
+class Zidovi:
+    def __init__(self, devicename: str = '/dev/ttyUSB0'):
+        self.devicename = devicename
+        self.portHandler = None
+        self.packetHandler = None
+
+    def _open_port(self):
+        self.portHandler = PortHandler(self.devicename)
+        self.packetHandler = sts(self.portHandler)
+        if self.portHandler.openPort():
+            print("Succeeded to open the port")
+        else:
+            print("Failed to open the port")
+            raise RuntimeError(f"Failed to open port: {self.devicename}")
+
+    def _close_port(self):
+        if self.portHandler is not None:
+            for motor_id in (3, 5, 2, 4):
+                stopped = False
+                while not stopped:
+                    try:
+                        stop_motor(self.packetHandler, motor_id)
+                        stopped = True
+                    except Exception:
+                        self.portHandler.ser.reset_input_buffer()
+                        self.portHandler.ser.reset_output_buffer()
+                        time.sleep(0.01)
+            self.portHandler.closePort()
+
+    def _stop_all(self):
+        # Reset serial buffers — a KeyboardInterrupt can fire mid-packet,
+        # leaving garbage in the buffers that would corrupt subsequent writes.
+        if self.portHandler is not None and self.portHandler.ser is not None:
+            self.portHandler.ser.reset_input_buffer()
+            self.portHandler.ser.reset_output_buffer()
+        for motor_id in (2, 3, 4, 5):
+            stopped = False
+            while not stopped:
+                try:
+                    stop_motor(self.packetHandler, motor_id)
+                    stopped = True
+                except Exception:
+                    self.portHandler.ser.reset_input_buffer()
+                    self.portHandler.ser.reset_output_buffer()
+                    time.sleep(0.01)
+
+    def zidovi(self, smer, brzina=1500,
+               TargetPos_beli=600, TargetPos_plavi=300,
+               opterecenje_threshold_beli=120, opterecenje_threshold_plavi=150):
+        TargetPos_beli = TargetPos_beli * 4096 / 360
+        TargetPos_plavi = TargetPos_plavi * 4096 / 360
+
+        if smer == 1:
+            Speed_ID2 = brzina
+            Speed_ID4 = -brzina
+            Speed_ID3 = -brzina
+            Speed_ID5 = brzina
+        else:
+            Speed_ID2 = -brzina
+            Speed_ID4 = brzina
+            Speed_ID3 = brzina
+            Speed_ID5 = -brzina
+
+        self._open_port()
+        try:
+            motor2 = CrniMotor(self.packetHandler, 2)
+            motor3 = CrniMotor(self.packetHandler, 3)
+            motor4 = CrniMotor(self.packetHandler, 4)
+            motor5 = CrniMotor(self.packetHandler, 5)
+
+            brojac = 0
+
+
+            load_blue_count = 0
+            load_white_count = 0
+
+            while True:
+                curr_time = time.time()
+
+                if smer == 1:
+                    motor3.doCycle(TargetPos_beli, Speed_ID3)
+                    motor5.doCycle(TargetPos_beli, Speed_ID5)
+                    if brojac >= 50:
+                        motor2.doCycle(TargetPos_plavi, Speed_ID2)
+                        motor4.doCycle(TargetPos_plavi, Speed_ID4)
+                    else:
+                        brojac += 1
+                else:
+                    motor2.doCycle(TargetPos_plavi, Speed_ID2)
+                    motor4.doCycle(TargetPos_plavi, Speed_ID4)
+                    if brojac >= 50:
+                        motor3.doCycle(TargetPos_beli, Speed_ID3)
+                        motor5.doCycle(TargetPos_beli, Speed_ID5)
+                    else:
+                        brojac += 1
+
+                print("Time elapsed: %.4f seconds" % (time.time() - curr_time))
+
+                opterecenje2 = get_load(self.packetHandler, 2)
+                opterecenje4 = get_load(self.packetHandler, 4)
+                opterecenje3 = get_load(self.packetHandler, 3)
+                opterecenje5 = get_load(self.packetHandler, 5)
+
+                napon2 = get_current(self.packetHandler, 2)
+                napon4 = get_current(self.packetHandler, 4)
+                napon3 = get_current(self.packetHandler, 3)
+                napon5 = get_current(self.packetHandler, 5)
+
+                print("Current positions:")
+                print("[ID:002] PresPos:%d Load:%.1f Current:%.1f" % (motor2.prosla_pos, opterecenje2, napon2))
+                print("[ID:004] PresPos:%d Load:%.1f Current:%.1f" % (motor4.prosla_pos, opterecenje4, napon4))
+                print("[ID:003] PresPos:%d Load:%.1f Current:%.1f" % (motor3.prosla_pos, opterecenje3, napon3))
+                print("[ID:005] PresPos:%d Load:%.1f Current:%.1f" % (motor5.prosla_pos, opterecenje5, napon5))
+
+                if napon2 > opterecenje_threshold_plavi or napon4 > opterecenje_threshold_plavi:
+
+                    load_blue_count+=1
+                    if load_blue_count > 2:
+                        print("High load detected: [ID:002] Load:%.1f [ID:004] Load:%.1f" % (opterecenje2, opterecenje4))
+                        stop_motor(self.packetHandler, 2)
+                        stop_motor(self.packetHandler, 4)
+                        motor2.targret_reached = True
+                        motor4.targret_reached = True
+                else:
+                    load_blue_count = max(0, load_white_count - 1)
+
+                if napon3 > opterecenje_threshold_beli or napon5 > opterecenje_threshold_beli:
+                    load_white_count+=1
+                    if load_white_count > 2:
+                        print("High load detected: [ID:003] Load:%.1f [ID:005] Load:%.1f" % (opterecenje3, opterecenje5))
+                        stop_motor(self.packetHandler, 3)
+                        stop_motor(self.packetHandler, 5)
+                        motor3.targret_reached = True
+                        motor5.targret_reached = True
+                else:
+                    load_white_count = max(0, load_white_count - 1)
+
+                print("PredjeniPut ID2: %d deg" % (motor2.predjeni_put * 360 / 4096))
+                print("PredjeniPut ID4: %d deg" % (motor4.predjeni_put * 360 / 4096))
+                print("PredjeniPut ID3: %d deg" % (motor3.predjeni_put * 360 / 4096))
+                print("PredjeniPut ID5: %d deg" % (motor5.predjeni_put * 360 / 4096))
+
+                targets_reached = (
+                    motor2.targret_reached
+                    and motor3.targret_reached
+                    and motor4.targret_reached
+                    and motor5.targret_reached
+                )
+                if targets_reached:
+                    print("Svi motori su stigli na cilj!")
+                    break
+
+            return smer, motor2.predjeni_put, motor3.predjeni_put, motor4.predjeni_put, motor5.predjeni_put
+        except KeyboardInterrupt:
+            print("\nCtrl+C detected, stopping all motors.")
+            self._stop_all()
+        finally:
+            self._close_port()

toid_lidar/launch/launch.py

@@ -7,6 +7,7 @@ from launch_ros.substitutions import FindPackageShare
 
 from pathlib import Path
 
+
 def generate_launch_description():
 
     basedir = FindPackageShare("").find("toid_lidar")
@@ -19,50 +20,48 @@ def generate_launch_description():
     use_closest = LaunchConfiguration("use_closest")
     lidar_frame = LaunchConfiguration("lidar_frame")
 
-    return LaunchDescription([
+    return LaunchDescription(
+        [
             DeclareLaunchArgument(
-            'visualize',
-            default_value='False',
-            description="Whether to launch rviz2" 
+                "visualize",
+                default_value="False",
+                description="Whether to launch rviz2",
             ),
             DeclareLaunchArgument(
-            'draw_markers',
-            default_value='False',
-            description="Draw markers" 
+                "draw_markers", default_value="False", description="Draw markers"
             ),
             DeclareLaunchArgument(
-            'use_closest',
-            default_value='True',
-            description="Use closest point for calibration" 
+                "use_closest",
+                default_value="True",
+                description="Use closest point for calibration",
             ),
             DeclareLaunchArgument(
-            'lidar_frame',
-            default_value='lidar_frame',
-            description="TF frame of the lidar" 
+                "lidar_frame",
+                default_value="lidar_frame",
+                description="TF frame of the lidar",
             ),
             Node(
-            package='toid_lidar',
-            executable='toid_lidar',
+                package="toid_lidar",
+                executable="toid_lidar",
                 output="screen",
                 parameters=[
                     lidar_config,
-                {
-                    'closest': use_closest, 
-                    'draw_markers': draw_markers
-                }]
+                    {"closest": use_closest, "draw_markers": draw_markers},
+                ],
             ),
             Node(
-            package='rplidar_ros',
-            executable='rplidar_composition',
+                package="rplidar_ros",
+                executable="rplidar_composition",
                 output="screen",
-            parameters=[lidar_config, {'frame_id': lidar_frame}]
+                parameters=[lidar_config, {"frame_id": lidar_frame}],
             ),
             Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz2',
-            output='screen',
-            arguments=['-d', rviz_config],
-            condition=IfCondition(visualize)
+                package="rviz2",
+                executable="rviz2",
+                name="rviz2",
+                output="screen",
+                arguments=["-d", rviz_config],
+                condition=IfCondition(visualize),
+            ),
+        ]
     )
-    ])

toid_lidar/src/toid_lidar_node.cpp

@@ -47,7 +47,7 @@ void ToidRivalDetect::process_scan(LaserScan::ConstSharedPtr msg)
   geometry_msgs::msg::TransformStamped transform;
 
   try {
-    transform = tf_buf_->lookupTransform(global_frame_, msg->header.frame_id, msg->header.stamp);
+    transform = tf_buf_->lookupTransform(global_frame_, msg->header.frame_id, tf2::TimePointZero);
   } catch (const tf2::TransformException & e) {
     RCLCPP_WARN_THROTTLE(
       this->get_logger(), *this->get_clock(), 1000, "TF Link missing: %s", e.what());

toid_msgs/CMakeLists.txt

@@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "action/SimpleMoveCoords.action"
   "action/SimpleRotate.action"
   "action/SimpleTranslateX.action"
+  "action/EmptyAction.action"
   DEPENDENCIES geometry_msgs
 )
 

toid_msgs/action/EmptyAction.action

@@ -0,0 +1,2 @@
+---
+---

toid_navigation/launch/main.py

@@ -1,7 +1,7 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.conditions import IfCondition, UnlessCondition 
-from launch.substitutions import  LaunchConfiguration
+from launch.substitutions import  LaunchConfiguration, AllSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -10,6 +10,8 @@ import os
 def generate_launch_description():
     nav_pkg_share = FindPackageShare("").find('toid_navigation')
     control_pkg_share = FindPackageShare("").find('toid_control')
+    lidar_pkg_share = FindPackageShare("").find('toid_lidar')
+    vision_pkg_share = FindPackageShare("").find('toid_vision')
     params = os.path.join(nav_pkg_share, 'params', 'toid_general_params.yaml')
     map = os.path.join(nav_pkg_share, 'maps', 'mapb2_5cm.yaml')
     ctrl_launch_dir = os.path.join(control_pkg_share, 'launch')
@@ -38,6 +40,20 @@ def generate_launch_description():
         description="Whether to launch rviz2" 
     )
 
+    use_lidar = LaunchConfiguration("use_lidar")
+    use_lidar_arg = DeclareLaunchArgument(
+        'use_lidar',
+        default_value='True',
+        description="Whether to launch rviz2" 
+    )
+
+    is_blue = LaunchConfiguration("is_blue")
+    is_blue_arg = DeclareLaunchArgument(
+        'is_blue',
+        default_value='True',
+        description="Whether to launch rviz2" 
+    )
+    
     toid_control = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(ctrl_launch_dir, 'toid.launch.py')
@@ -49,6 +65,35 @@ def generate_launch_description():
         condition=IfCondition(run_nodes),
     )
 
+    toid_lidar = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_pkg_share, 'launch' , 'launch.py')
+        ),
+        launch_arguments={
+            'visualize': 'False',
+            'lidar_frame': 'lidar', 
+        }.items(),
+        condition=IfCondition(AllSubstitution(run_nodes, use_lidar)),
+    ) 
+
+    toid_vision = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(vision_pkg_share, 'launch' , 'launch.py')
+        ),
+        launch_arguments={
+            'is_blue': is_blue,
+        }.items(),
+        condition=IfCondition(AllSubstitution(run_nodes, use_lidar)),
+    ) 
+
+    toid_interaction = Node(
+        package='toid_interaction',
+        executable='node',
+        name='toid_interaction',
+        output='screen',
+        condition=IfCondition(run_nodes),
+    )
+
     map_server = Node(
         package='nav2_map_server',
         executable='map_server',
@@ -118,6 +163,11 @@ def generate_launch_description():
         visualize_arg,
         use_mock_arg,
         run_nodes_arg,
+        use_lidar_arg,
+        is_blue_arg,
+        toid_lidar,
+        toid_vision,
+        toid_interaction,
         rviz_node,
         map_server,
         bt_navigator,

toid_navigation/params/toid_general_params.yaml

@@ -22,19 +22,28 @@ behavior_server:
     local_footprint_topic: local_costmap/published_footprint
     global_footprint_topic: global_costmap/published_footprint
     cycle_frequency: 50.0
-    behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords"]
+    behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords", "approachAcorns", "rotateAcorns"]
     spin:
       plugin: "nav2_behaviors::Spin"
     backup:
       plugin: "nav2_behaviors::BackUp"
     rotate:
       plugin: "toid::SimpleRotateBehavior"
-      max_angular_accel: 4.0
+      max_angular_accel: 3.0
       max_angular_decel: 1.0
     translateX:
       plugin: "toid::SimpleTranslateXBehavior"
     moveCoords:
       plugin: "toid::MoveCoords"
+      max_vel_accel: 1.0
+    approachAcorns:
+      plugin: "toid::ApproachAcorns"
+      kphi: 1.2
+      kdelta: 2.0
+      lambda: 3.0
+      debug_marker: true
+    rotateAcorns:
+      plugin: "toid::RotateAcorns"
     local_frame: map
     global_frame: map
     robot_base_frame: base_footprint
@@ -52,7 +61,7 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_footprint
-      footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]"
+      footprint: "[[-0.045, 0.15], [0.255, 0.15], [0.255, -0.15], [-0.045, -0.15]]"
       footprint_padding: 0.02
       track_unknown_space: false
       rolling_window: false
@@ -73,7 +82,7 @@ local_costmap:
     ros__parameters:
       update_frequency: 5.0
       publish_frequency: 2.0
-      footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]"
+      footprint: "[[-0.045, 0.15], [0.255, 0.15], [0.255, -0.15], [-0.045, -0.15]]"
       footprint_padding: 0.01
       #robot_radius: 0.18
       global_frame: map
@@ -83,11 +92,14 @@ local_costmap:
       height: 1
       resolution: 0.01
       introspection_mode: "disabled"
-      plugins: ["static_layer", "inflation_layer"]
+      plugins: ["static_layer", "rival_layer", "inflation_layer"]
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
         footprint_clearing_enabled: true
         map_subscribe_transient_local: True
+      rival_layer:
+        plugin: "toid::RivalLayer"
+        rival_size: 0.15
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0

toid_navigation/rviz/nav2.rviz

@@ -1,13 +1,14 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 78
+    Help Height: 138
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /RobotModel1
         - /GlobalCostMap1/Topic1
+        - /Pose1
       Splitter Ratio: 0.5
-    Tree Height: 592
+    Tree Height: 668
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -74,6 +75,10 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         drivewhl_l_link:
           Alpha: 1
           Show Axes: false
@@ -247,6 +252,39 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /start_point
       Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: Marker
+      Namespaces:
+        "": true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /marker
+      Value: true
+    - Alpha: 1
+      Axes Length: 0.10000000149011612
+      Axes Radius: 0.019999999552965164
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      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
@@ -293,33 +331,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/XYOrbit
-      Distance: 3.863811731338501
+      Distance: 2.2019217014312744
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.1778966784477234
-        Y: -1.1747734546661377
-        Z: 1.7285346984863281e-06
+        X: 0.3222081661224365
+        Y: -0.08033189922571182
+        Z: 0
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.3697962760925293
+      Pitch: 1.064796805381775
       Target Frame: <Fixed Frame>
       Value: XYOrbit (rviz_default_plugins)
-      Yaw: 4.628584861755371
+      Yaw: 4.925206184387207
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 896
+  Height: 1186
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000254000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002de000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002de000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005d30000003efc0100000002fb0000000800540069006d00650100000000000005d30000027b00fffffffb0000000800540069006d0065010000000000000450000000000000000000000264000002de00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000025400000398fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000003980000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b00000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000003980000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070c0000005efc0100000002fb0000000800540069006d006501000000000000070c0000048700fffffffb0000000800540069006d00650100000000000004500000000000000000000003450000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -328,6 +366,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1491
-  X: 429
-  Y: 75
+  Width: 1804
+  X: 428
+  Y: 74

toid_odometry/src/odometry.cpp

@@ -139,6 +139,23 @@ private:
         tf_static_broadcaster_->sendTransform(t);
     }
 
+    void serial_ec_recovery(boost::system::error_code ec) {
+        if(ec.failed()) {
+            boost::system::error_code ec;
+            ec = this->serial_.close(ec);
+            ec = this->serial_.open(serial_port_path_, ec);
+            if(!ec.failed()) {
+                try {
+                    for(int i = 0; i < 64; i++) {
+                        asio::write(this->serial_, asio::buffer("\x00"));
+                    }
+                } catch(const std::runtime_error& err) {
+                    
+                }
+            }
+        }
+    }
+
     void set_width(const std::shared_ptr<SendDoubleSrv::Request> req) {
         boost::system::error_code ec;
         const std::array<uint8_t, 2> cmd{TMSG_SET_WIDTH, TMSG_DELIM};
@@ -146,6 +163,7 @@ private:
         msg.crc = crcFooter(msg);
         asio::write(this->serial_, asio::buffer(cmd), ec);
         asio::write(this->serial_, asio::buffer(&msg, sizeof(msg)), ec);
+        serial_ec_recovery(ec);
     }
 
     void set_ratio(const std::shared_ptr<SendDoubleSrv::Request> req) {
@@ -155,6 +173,7 @@ private:
         msg.crc = crcFooter(msg);
         asio::write(this->serial_, asio::buffer(cmd), ec);
         asio::write(this->serial_, asio::buffer(&msg, sizeof(msg)), ec);
+        serial_ec_recovery(ec);
     }
 
     void end_calib(const std::shared_ptr<ZeroSrv::Request>) {
@@ -166,12 +185,15 @@ private:
         if(!ec.failed()) {
             RCLCPP_INFO(this->get_logger(), "Calculated ratio: %lf, %lf", resp.left_gain, resp.right_gain);
         }
+        serial_ec_recovery(ec);
+
     }
 
     void zero(const std::shared_ptr<ZeroSrv::Request>) {
         boost::system::error_code ec;
         const std::array<uint8_t, 2> cmd{TMSG_ZERO, TMSG_DELIM};
         asio::write(this->serial_, asio::buffer(cmd), ec);
+        serial_ec_recovery(ec);
     }
 
     void publish_robot_state() {
@@ -203,6 +225,7 @@ private:
         o.child_frame_id = t.child_frame_id;
 
         t.transform = make_transform(state.x, state.y, state.theta);
+        RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 500, "Robot position: %lf %lf %lf", state.x, state.y, state.theta);
 
         const auto& [x,y,z] = t.transform.translation;
         o.pose.pose.position.x = x;
@@ -244,6 +267,17 @@ private:
                     ec.what().c_str()
                 );
             }
+
+            if(!ec.failed()) {
+                try {
+                    for(int i = 0; i < 64; i++) {
+                        asio::write(serial_, asio::buffer("\x00"));
+                    }
+                } catch(const std::runtime_error& err) {
+                    
+                }
+            }
+
             return false;
         }
         return state.crc == crcFooter(state);

toid_vision/CMakeLists.txt

@@ -0,0 +1,97 @@
+
+cmake_minimum_required(VERSION 3.8)
+project(toid_vision)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+
+set(PACKAGE_DEPS
+  rclcpp
+  sensor_msgs
+  message_filters
+  cv_bridge
+  OpenCV
+  rclcpp_components
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+)
+
+foreach(PACKAGE ${PACKAGE_DEPS})
+  find_package(${PACKAGE} REQUIRED)
+endforeach()
+
+find_package(OpenCV 4 REQUIRED 
+  COMPONENTS 
+    opencv_core 
+    opencv_aruco
+    opencv_imgproc 
+    opencv_imgcodecs
+)
+
+
+set(SOURCES
+    src/toid_vision.cpp
+    src/vision.cpp
+)
+
+set(SOURCES_COMPOSABLE
+    src/toid_vision.cpp
+)
+
+add_executable(toid_vision ${SOURCES})
+
+target_include_directories(
+  toid_vision
+  PRIVATE
+  include
+)
+
+ament_target_dependencies(toid_vision ${PACKAGE_DEPS})
+
+install(TARGETS toid_vision DESTINATION lib/${PROJECT_NAME})
+
+# TOID_VISION COMPOSABLE NODE
+
+add_library(toid_vision_component SHARED ${SOURCES_COMPOSABLE})
+
+rclcpp_components_register_node(
+    toid_vision_component
+    PLUGIN "toid::NutDetector"
+    EXECUTABLE nut_detector 
+)
+
+target_include_directories(
+  toid_vision_component
+  PRIVATE
+  include
+)
+
+ament_target_dependencies(toid_vision_component ${PACKAGE_DEPS})
+
+ament_export_targets(export_toid_vision_component)
+install(TARGETS toid_vision_component
+        EXPORT export_toid_vision_component
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY 
+    launch 
+    config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+target_compile_features(
+  toid_vision PUBLIC
+  c_std_99
+  cxx_std_17
+)
+
+
+ament_package()

toid_vision/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_vision/config/camera_info.yaml

@@ -0,0 +1,27 @@
+image_width: 1640
+image_height: 1232
+camera_name: imx219__base_axi_pcie_1000120000_rp1_i2c_88000_imx219_10_1640x1232
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [1281.9678810697528, 0.0, 792.9357401565763,
+         0.0, 1283.1760537785433, 615.1915708814056,
+         0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [0.19247275241900028, -0.5098314224324741, 6.710711247494001e-05,
+         -0.00018449989636263572, 0.40356075592228546]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.0, 0.0, 0.0,
+         0.0, 1.0, 0.0,
+         0.0, 0.0, 1.0]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [1281.9678810697528, 0.0, 792.9357401565763, 0.0,
+         0.0, 1283.1760537785433, 615.1915708814056, 0.0,
+         0.0, 0.0, 1.0, 0.0]

toid_vision/include/toid_vision/toid_vision.hpp

@@ -0,0 +1,71 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include "message_filters/subscriber.hpp"
+#include "message_filters/sync_policies/approximate_time.hpp"
+#include "message_filters/time_synchronizer.hpp"
+#include "opencv2/aruco.hpp"
+#include "opencv2/objdetect.hpp"
+#include "opencv2/opencv.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/camera_info.hpp"
+#include "sensor_msgs/msg/compressed_image.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "tf2/LinearMath/Quaternion.hpp"
+#include "tf2/LinearMath/Transform.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/buffer.hpp"
+
+namespace toid
+{
+
+class NutDetector : public rclcpp::Node
+{
+  using SyncPolicy = message_filters::sync_policies::ApproximateTime<
+    sensor_msgs::msg::CompressedImage, sensor_msgs::msg::CameraInfo>;
+
+public:
+  NutDetector(const rclcpp::NodeOptions & options);
+
+public:
+  void compressed_topic_callback(
+    const sensor_msgs::msg::CompressedImage::ConstSharedPtr & msg,
+    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info);
+
+  void process_image(
+    cv::Mat & decodedImage, const std_msgs::msg::Header & header,
+    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info);
+
+  void extract_markers(cv::Mat & image);
+
+  void get_pose(cv::Vec3d & tvec, cv::Vec3d & rvec, geometry_msgs::msg::Pose & out_pose);
+
+  message_filters::Subscriber<sensor_msgs::msg::CompressedImage> compressed_img_sub_;
+  message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cam_info_sub_;
+  std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_;
+
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr flip_pub_;
+
+  std::vector<std::vector<cv::Point2f>> markerCorners_;
+  std::vector<int> markerIds_;
+  std::vector<std::pair<std::vector<cv::Point2f>, int>> markers_;
+
+  std::string camera_frame_id_ = "camera";
+  bool is_blue_;
+
+  cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
+  cv::Ptr<cv::aruco::Dictionary> dictionary_;
+
+  message_filters::Subscriber<sensor_msgs::msg::Image> img_sub_;
+
+  tf2_ros::Buffer::SharedPtr tf_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+};
+
+}  // namespace toid

toid_vision/launch/launch.py

@@ -0,0 +1,55 @@
+import launch
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration 
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
+import os
+
+def generate_launch_description():
+    """Generate launch description with multiple components."""
+
+    vision_share = FindPackageShare("").find("toid_vision")
+    camera_info = os.path.join(vision_share, 'config/camera_info.yaml')
+
+    is_blue_arg = DeclareLaunchArgument(
+        name= "is_blue",
+        default_value = "True"
+    ) 
+
+    is_blue = LaunchConfiguration("is_blue")
+
+    container = ComposableNodeContainer(
+            name='vision_container',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container_mt',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='toid_vision',
+                    plugin='toid::NutDetector',
+                    name='nut_detector',
+                    parameters= [{
+                        'is_blue': is_blue,
+                    }]),
+                ComposableNode(
+                    package='camera_ros',
+                    plugin='camera::CameraNode',
+                    name='camera',
+                    parameters= [{
+                        'width': 1640,
+                        'height': 1242,
+                        'orientation': 180,
+                        'camera': 0,
+                        'frame_id': 'camera',
+                        'camera_info_url': "file://" + camera_info
+                    }],
+                    )
+            ],
+            output='screen',
+    )
+
+    return launch.LaunchDescription([
+        is_blue_arg,
+        container
+        ])

toid_vision/package.xml

@@ -0,0 +1,26 @@
+<?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_vision</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>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>cv_bridge</depend>
+  <depend>libopencv-dev</depend>
+  <depend>rclcpp_components</depend>
+
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

toid_vision/src/toid_vision.cpp

@@ -0,0 +1,203 @@
+#include "toid_vision/toid_vision.hpp"
+
+#include <string>
+#include <vector>
+#include "tf2/utils.hpp"
+
+namespace toid
+{
+
+NutDetector::NutDetector(const rclcpp::NodeOptions & options)
+: Node("compressed_image_subscriber", options)
+{
+  detectorParams_ = cv::aruco::DetectorParameters::create();
+  dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_100);
+
+  tf_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
+
+  bool use_compressed = true;
+  this->declare_parameter("use_compressed", rclcpp::ParameterValue(true));
+  this->get_parameter("use_compressed", use_compressed);
+
+  this->declare_parameter("camera_frame_id", rclcpp::ParameterValue("camera"));
+  this->get_parameter("camera_frame_id", camera_frame_id_);
+
+  this->declare_parameter("is_blue", rclcpp::ParameterValue(true));
+  this->get_parameter("is_blue", is_blue_);
+
+  compressed_img_sub_.subscribe(this, "/camera/image_raw/compressed");
+  cam_info_sub_.subscribe(this, "/camera/camera_info");
+
+  sync_ = std::make_shared<message_filters::Synchronizer<SyncPolicy>>(
+    SyncPolicy(2), compressed_img_sub_, cam_info_sub_);
+
+  using namespace std::placeholders;
+  sync_->registerCallback(std::bind(&NutDetector::compressed_topic_callback, this, _1, _2));
+  RCLCPP_INFO(this->get_logger(), "NutDetector activated... scanning for DEEZ NUTZ");
+
+  pose_pub_ =
+    this->create_publisher<geometry_msgs::msg::PoseStamped>("/closest_acorn", rclcpp::QoS(1).keep_last(1));
+  flip_pub_ = this->create_publisher<std_msgs::msg::String>("/to_flip", rclcpp::QoS(1));
+}
+
+void NutDetector::compressed_topic_callback(
+  const sensor_msgs::msg::CompressedImage::ConstSharedPtr & msg,
+  const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info)
+{
+  try {
+    cv::Mat rawData(1, msg->data.size(), CV_8UC1, (void *)msg->data.data());
+    cv::Mat decodedImage = cv::imdecode(rawData, cv::IMREAD_COLOR);
+
+    if (decodedImage.empty()) {
+      RCLCPP_ERROR(this->get_logger(), "Could not decode image!");
+      return;
+    }
+
+    process_image(decodedImage, msg->header, cam_info);
+
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(this->get_logger(), "Error processing image: %s", e.what());
+  }
+}
+
+void NutDetector::process_image(
+  cv::Mat & decodedImage, const std_msgs::msg::Header & header,
+  const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info)
+{
+  markers_.clear();
+
+  extract_markers(decodedImage);
+
+  if (markers_.empty()) {
+    return;
+  }
+
+  std::vector<cv::Vec3d> rvecs, tvecs;
+
+  cv::Mat k = cv::Mat(3, 3, CV_64F, (void *)cam_info->k.data());
+  cv::aruco::estimatePoseSingleMarkers(markerCorners_, 0.03, k, cam_info->d, rvecs, tvecs);
+
+  size_t prev = 0;
+  std::string colors = "";
+  size_t first_ok = -1;
+
+  for (size_t i = 0; i < markerIds_.size(); i++) {
+    const int idx = markers_[i].second;
+    if (markerIds_[idx] != 47 && markerIds_[idx] != 36) {
+      continue;
+    }
+    if (first_ok == (size_t)-1) {
+      first_ok = idx;
+    }
+
+    RCLCPP_DEBUG(
+      this->get_logger(), "Marker %ld at %lf %lf", i, markerCorners_[idx][0].x,
+      markerCorners_[idx][0].y);
+
+    RCLCPP_DEBUG(
+      this->get_logger(), "World coords: %lf %lf %lf", tvecs[idx][0], tvecs[idx][1], tvecs[idx][2]);
+
+    float dx = tvecs[idx][0] - tvecs[prev][0];
+    float dy = tvecs[idx][1] - tvecs[prev][1];
+    float dz = tvecs[idx][2] - tvecs[prev][2];
+
+    float dist = std::sqrt(dx * dx + dy * dy + dz * dz);
+
+    RCLCPP_DEBUG(this->get_logger(), "Dist from prev: %lf", dist);
+
+    if (dist > 0.07) {
+      continue;
+    }
+
+    prev = idx;
+
+    colors += (markerIds_[idx] == 36) ? "b" : "y";
+
+    if (colors.size() > 3) {
+      break;
+    }
+  }
+
+  if (colors.size() == 4) {
+    RCLCPP_DEBUG(this->get_logger(), "Last seen orientation: %s", colors.c_str());
+    for (int i = 0; i < 4; i++) {
+      if (colors[i] == 'b') {
+        colors[i] = (is_blue_) ? '0' : '1';
+      } else {
+        colors[i] = (is_blue_) ? '1' : '0';
+      }
+    }
+
+    RCLCPP_DEBUG(this->get_logger(), "Last seen orientation: %s", colors.c_str());
+    std_msgs::msg::String m;
+    m.data = colors;
+    flip_pub_->publish(m);
+  }
+
+  if (first_ok == (size_t)-1) {
+    return;
+  }
+
+  geometry_msgs::msg::PoseStamped pose;
+  get_pose(tvecs[markers_[0].second], rvecs[markers_[0].second], pose.pose);
+  pose.header.frame_id = camera_frame_id_;
+  pose.header.stamp = header.stamp;
+  pose_pub_->publish(pose);
+
+  try {
+    geometry_msgs::msg::PoseStamped out_pose;
+    out_pose = tf_->transform(pose, "base_footprint");
+    RCLCPP_DEBUG_THROTTLE(
+      this->get_logger(), *this->get_clock(), 2000, "Closest guy is at: %lf %lf %lf",
+      out_pose.pose.position.x, out_pose.pose.position.y, tf2::getYaw(out_pose.pose.orientation)
+    );
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_ERROR_THROTTLE(
+      this->get_logger(), *this->get_clock(), 2000, "Failed to transfrom point");
+  }
+}
+
+void NutDetector::extract_markers(cv::Mat & image)
+{
+  cv::aruco::detectMarkers(image, dictionary_, markerCorners_, markerIds_, detectorParams_);
+
+  // cv::aruco::drawDetectedMarkers(image, markerCorners_, markerIds_);
+
+  markers_.reserve(markerIds_.size());
+  markers_.resize(markerIds_.size());
+
+  for (size_t i = 0; i < markerIds_.size(); i++) {
+    markers_[i].first = markerCorners_[i];
+    markers_[i].second = i;
+  }
+
+  std::sort(
+    markers_.begin(), markers_.end(),
+    [](std::pair<std::vector<cv::Point2f>, int> & a, std::pair<std::vector<cv::Point2f>, int> & b) {
+      return a.first[0].y > b.first[0].y;
+    });
+}
+
+void NutDetector::get_pose(cv::Vec3d & tvec, cv::Vec3d & rvec, geometry_msgs::msg::Pose & out_pose)
+{
+  cv::Mat R;
+  cv::Rodrigues(rvec, R);
+  tf2::Matrix3x3 tf2_rot{R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
+                         R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
+                         R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2)};
+
+  tf2::Quaternion q;
+  tf2_rot.getRotation(q);
+
+  tf2::convert(q, out_pose.orientation);
+
+  out_pose.position.x = tvec[0];
+  out_pose.position.y = tvec[1];
+  out_pose.position.z = tvec[2];
+}
+
+}  // namespace toid
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(toid::NutDetector)

toid_vision/src/vision.cpp

@@ -0,0 +1,10 @@
+#include "toid_vision/toid_vision.hpp"
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions options;
+  rclcpp::spin(std::make_shared<toid::NutDetector>(options));
+  rclcpp::shutdown();
+  return 0;
+}