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,23 +1,258 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
+  <BehaviorTree ID="pickup_crates">
+    <Seq1 service_name=""/>
+  </BehaviorTree>
+
+  <BehaviorTree ID="seq">
+    <Sequence>
+      <InPose timeout="1.000000">
+        <Sleep msec="999999"/>
+      </InPose>
+      <Seq1 service_name=""/>
+      <Seq2 text="1010"
+            service_name=""/>
+      <Seq3 service_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
   <BehaviorTree ID="seq1">
     <Sequence>
       <ZeroOdom service_name=""/>
-      <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>
 
@@ -26,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=""/>
@@ -40,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"/>
@@ -61,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"/>
@@ -70,6 +314,37 @@
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
     </Action>
+    <Action ID="Seq1">
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
+    <Action ID="Seq2">
+      <input_port name="text"
+                  type="std::string"/>
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
+    <Action ID="Seq3">
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
+    <Condition ID="SetInitialPose">
+      <input_port name="y"
+                  default="0.000000"
+                  type="double">Y position in meters</input_port>
+      <input_port name="theta"
+                  default="0.000000"
+                  type="double">Heading in degrees</input_port>
+      <input_port name="frame_id"
+                  default="map"
+                  type="std::string">Reference frame</input_port>
+      <input_port name="x"
+                  default="0.000000"
+                  type="double">X position in meters</input_port>
+      <input_port name="topic_name"
+                  default="__default__placeholder__"
+                  type="std::string">Topic name</input_port>
+    </Condition>
     <Action ID="TranslateX">
       <input_port name="x"
                   type="double"/>
@@ -79,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,21 +23,21 @@
         <Sleep msec="500"/>
         <RotateSimple angle="0"
                       max_speed="0.500000"
-                      min_angle="270"
+                      min_angle="700"
                       action_name=""/>
       </Sequence>
       <ForceSuccess>
-        <Timeout msec="13000">
+        <DetectStuck timeout="1.000000">
           <MovePointSimple x="-1.0"
                            y="0"
                            theta="0"
                            max_speed="0.070000"
                            backwards="true"
                            action_name=""/>
-        </Timeout>
+        </DetectStuck>
       </ForceSuccess>
       <SetWidth width="{width}"
-                count="1"
+                count="2"
                 new_width="{width}"
                 service_name=""/>
     </Sequence>
@@ -46,40 +47,66 @@
     <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>
-        <Timeout msec="9000">
+        <DetectStuck timeout="1.000000">
           <MovePointSimple x="-0.2"
                            y="0"
                            theta="0"
                            max_speed="0.05"
                            backwards="true"
                            action_name=""/>
-        </Timeout>
+        </DetectStuck>
       </ForceSuccess>
       <Sleep msec="1000"/>
       <EndCalib service_name=""/>
@@ -88,6 +115,7 @@
 
   <BehaviorTree ID="wheel_size">
     <Sequence>
+      <WaitPullPin />
       <Sleep msec="1000"/>
       <ZeroOdom service_name=""/>
       <Sleep msec="1000"/>
@@ -102,6 +130,11 @@
 
   <!-- Description of Node Models (used by Groot) -->
   <TreeNodesModel>
+    <Decorator ID="DetectStuck">
+      <input_port name="timeout"
+                  default="1.000000"
+                  type="double"/>
+    </Decorator>
     <Action ID="EndCalib">
       <input_port name="service_name"
                   type="std::string">Service name</input_port>

toid_bt/behavior_trees/toid_behaviors.btproj

@@ -4,46 +4,89 @@
     <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" type="double" default="1.000000"/>
+            <input_port name="timeout" default="1.000000" type="double"/>
         </Decorator>
         <Action ID="EndCalib">
-            <input_port name="service_name" type="std::string" default="">Service name</input_port>
+            <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
+        <Decorator ID="InPose">
+            <input_port name="timeout" default="1.000000" type="double"/>
+        </Decorator>
         <Action ID="MovePointSimple">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
             <input_port name="theta" type="double"/>
-            <input_port name="max_speed" type="double" default="0.000000"/>
-            <input_port name="backwards" type="bool" default="false"/>
-            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="backwards" default="false" type="bool"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="RotateAcorns">
+            <input_port name="angle" type="double"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="min_angle" default="0.000000" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
         </Action>
         <Action ID="RotateSimple">
             <input_port name="angle" type="double"/>
-            <input_port name="max_speed" type="double" default="0.000000"/>
-            <input_port name="min_angle" type="double" default="0.000000"/>
-            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+            <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="RotateTowards">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
-            <input_port name="max_speed" type="double" default="0.000000"/>
-            <input_port name="min_angle" type="double" default="0.000000"/>
-            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+            <input_port name="backwards" default="false" type="bool"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="min_angle" default="0.000000" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="Seq1">
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+        <Action ID="Seq2">
+            <input_port name="text" type="std::string"/>
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+        <Action ID="Seq3">
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+        <Condition ID="SetInitialPose">
+            <input_port name="y" default="0.000000" type="double">Y position in meters</input_port>
+            <input_port name="theta" default="0.000000" type="double">Heading in degrees</input_port>
+            <input_port name="frame_id" default="map" type="std::string">Reference frame</input_port>
+            <input_port name="x" default="0.000000" type="double">X position in meters</input_port>
+            <input_port name="topic_name" default="__default__placeholder__" type="std::string">Topic name</input_port>
+        </Condition>
+        <Action ID="SetParameter">
+            <input_port name="parameter" type="std::string"/>
+            <input_port name="value" type="std::string"/>
+            <input_port name="node" type="std::string"/>
         </Action>
         <Action ID="SetWidth">
             <input_port name="width" type="double"/>
-            <input_port name="count" type="int" default="1"/>
+            <input_port name="count" default="1" type="int"/>
             <output_port name="new_width" type="double"/>
-            <input_port name="service_name" type="std::string" default="">Service name</input_port>
+            <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
         <Action ID="TranslateX">
             <input_port name="x" type="double"/>
-            <input_port name="max_speed" type="double" default="0.000000"/>
-            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+            <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" default="">Service name</input_port>
+            <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
     </TreeNodesModel>
 </root>

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

@@ -0,0 +1,48 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+
+#include "toid_msgs/srv/send_string.hpp"
+
+namespace toid
+{
+
+class SendTextNode : public BT::RosServiceNode<toid_msgs::srv::SendString>
+{
+public:
+  SendTextNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, FlipFunc get_text)
+  : BT::RosServiceNode<toid_msgs::srv::SendString>(name, conf, params), get_text(get_text)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<std::string>("text"),
+    });
+  }
+
+  bool setRequest(typename Request::SharedPtr &req) override { 
+    std::string text = getInput<std::string>("text").value_or("");
+    if(text=="") {
+      get_text(text);
+    }
+    req->text = text;
+
+    return true;
+  }
+
+  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+protected:
+  FlipFunc get_text;
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp

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

toid_bt/include/toid_bt/plugins/set_parameter_action.hpp

@@ -0,0 +1,50 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
+
+namespace toid
+{
+
+class SetParameterNode : public BT::RosServiceNode<rcl_interfaces::srv::SetParameters>
+{
+public:
+  SetParameterNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosServiceNode<rcl_interfaces::srv::SetParameters>(name, conf, params)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<std::string>("node"),
+      BT::InputPort<std::string>("parameter"),
+      BT::InputPort<std::string>("value")
+    });
+  }
+
+  bool setRequest(typename Request::SharedPtr & request) override { 
+    std::string parameter = getInput<std::string>("parameter").value();
+    std::string value = getInput<std::string>("value").value();
+    std::string node = getInput<std::string>("node").value();
+
+    setServiceName("/" + node + "/set_parameters");
+    request->parameters.emplace_back();
+    request->parameters.front().name = parameter;
+    request->parameters.front().value.string_value = value;
+    return true; 
+}
+
+  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+};
+
+}  // namespace

toid_bt/include/toid_bt/plugins/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,10 +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
@@ -31,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)
@@ -40,29 +52,62 @@ void TreeExecutor::onTreeCreated(BT::Tree & tree)
 
 void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
 {
+  FlipFunc get_to_flip = [this](std::string & pose) { pose = this->to_flip_; };
   PoseFunc get_pose = [this](geometry_msgs::msg::PoseStamped & pose) { this->position(pose); };
+  PoseFunc get_acorn_pose = [this](geometry_msgs::msg::PoseStamped & pose) {
+    this->acorn_position(pose);
+  };
+
   rclcpp::Node::SharedPtr nh = node();
   factory.registerNodeType<toid::MovePointNode>(
     "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
 
+  factory.registerNodeType<toid::MovePointNode>(
+    "ApproachAcorns", BT::RosNodeParams(nh, "/approachAcorns"), get_pose);
+
   factory.registerNodeType<toid::RotateNode>(
     "RotateSimple", BT::RosNodeParams(nh, "/rotate"), get_pose);
 
+  factory.registerNodeType<toid::RotateNode>(
+    "RotateAcorns", BT::RosNodeParams(nh, "/rotateAcorns"), get_pose);
+
   factory.registerNodeType<toid::RotateTowardsNode>(
     "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose);
 
   factory.registerNodeType<toid::TranslateXNode>(
-    "TranslateX", BT::RosNodeParams(nh, "/translateX"), get_pose);
+    "TranslateX", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
 
   factory.registerNodeType<toid::EndCalibNode>(
     "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose);
 
+  factory.registerNodeType<toid::PullPinNode>("WaitPullPin", BT::RosNodeParams(nh, "/start_plug"));
+
+  factory.registerNodeType<toid::SetParameterNode>(
+    "SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters"));
+
+  factory.registerNodeType<toid::SetInitialPoseNode>(
+    "SetInitialPose", BT::RosNodeParams(nh, "/initialpose"));
+
   factory.registerNodeType<toid::EmptySrvNode>("ZeroOdom", BT::RosNodeParams(nh, "/zero"));
 
   factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
 
+  BT::RosNodeParams service_params1(nh, "/sequence1");
+  service_params1.server_timeout = std::chrono::seconds(15);
+  factory.registerNodeType<toid::EmptySrvNode>("Seq1", service_params1);
+
+  BT::RosNodeParams service_params2(nh, "/sequence2");
+  service_params2.server_timeout = std::chrono::seconds(20);
+  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params2, get_to_flip);
+
+  BT::RosNodeParams service_params3(nh, "/sequence3");
+  service_params3.server_timeout = std::chrono::seconds(15);
+  factory.registerNodeType<toid::EmptySrvNode>("Seq3", service_params3);
+
   factory.registerNodeType<toid::StuckDetectorNode>("DetectStuck", get_pose);
 
+  factory.registerNodeType<toid::InPositionNode>("InPose", get_acorn_pose);
+
   std::cout << describeCustomNodes() << std::endl;
 }
 
@@ -71,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

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>toid_interaction</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="82343504+pimpest@users.noreply.github.com">pimpest</maintainer>
+  <license>MIT</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <depend>python3-serial</depend>
+  <depend>python3-gpiozero</depend>
+  <depend>toid_msgs</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

toid_interaction/setup.cfg

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

toid_interaction/setup.py

@@ -0,0 +1,33 @@
+from setuptools import find_packages, setup
+
+package_name = 'toid_interaction'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='pimpest',
+    maintainer_email='82343504+pimpest@users.noreply.github.com',
+    description='TODO: Package description',
+    license='MIT',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': [
+            'sequence = toid_interaction.mechanism.sekvenca_2026:main',
+            'node = toid_interaction.interaction_node:main',
+            'cam_calib = toid_interaction.camera:main',
+            'cam_calib1 = toid_interaction.camera1:main'
+        ],
+    },
+)

toid_interaction/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

toid_interaction/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

toid_interaction/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

toid_interaction/toid_interaction/interaction_node.py

@@ -0,0 +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")
+
+        self.find_sigma()
+
+        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 == 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:
+            return Empty.Response()
+        okreni(5)
+        zupcanik = ZupcanikAction(self.st_motor_device_name)
+        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 SendString.Response()
+
+        zidovi = Zidovi(self.st_motor_device_name)
+
+        zidovi.zidovi(1, 1500, 600, 210, 120, 120)
+
+        okreni_niz(request.text)
+
+        zidovi.zidovi(0, 1500, 420, 50, 120, 120)
+        self.step = 2
+        return response
+
+    def sequence3_cb(self, request, response):
+        if self.step != 2:
+            return Empty.Response()
+
+        zupcanik = ZupcanikAction(self.st_motor_device_name)
+        zidovi = Zidovi(self.st_motor_device_name)
+
+        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()
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(node)
+    executor.spin()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py

@@ -0,0 +1,8 @@
+#!/usr/bin/env python
+
+from .port_handler import *
+from .protocol_packet_handler import *
+from .group_sync_write import *
+from .group_sync_read import *
+from .sts import *
+from .scscl import *

toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py

@@ -0,0 +1,151 @@
+#!/usr/bin/env python
+
+from .stservo_def import *
+
+class GroupSyncRead:
+    def __init__(self, ph, start_address, data_length):
+        self.ph = ph
+        self.start_address = start_address
+        self.data_length = data_length
+
+        self.last_result = False
+        self.is_param_changed = False
+        self.param = []
+        self.data_dict = {}
+
+        self.clearParam()
+
+    def makeParam(self):
+        if not self.data_dict:  # len(self.data_dict.keys()) == 0:
+            return
+
+        self.param = []
+
+        for scs_id in self.data_dict:
+            self.param.append(scs_id)
+
+    def addParam(self, sts_id):
+        if sts_id in self.data_dict:  # sts_id already exist
+            return False
+
+        self.data_dict[sts_id] = []  # [0] * self.data_length
+
+        self.is_param_changed = True
+        return True
+
+    def removeParam(self, sts_id):
+        if sts_id not in self.data_dict:  # NOT exist
+            return
+
+        del self.data_dict[sts_id]
+
+        self.is_param_changed = True
+
+    def clearParam(self):
+        self.data_dict.clear()
+
+    def txPacket(self):
+        if len(self.data_dict.keys()) == 0:
+
+            return COMM_NOT_AVAILABLE
+
+        if self.is_param_changed is True or not self.param:
+            self.makeParam()
+
+        return self.ph.syncReadTx(self.start_address, self.data_length, self.param, len(self.data_dict.keys()))
+
+    def rxPacket(self):
+        self.last_result = True
+
+        result = COMM_RX_FAIL
+
+        if len(self.data_dict.keys()) == 0:
+            return COMM_NOT_AVAILABLE
+
+        result, rxpacket = self.ph.syncReadRx(self.data_length, len(self.data_dict.keys()))
+        # print(rxpacket)
+        if len(rxpacket) >= (self.data_length+6):
+            for sts_id in self.data_dict:
+                self.data_dict[sts_id], result = self.readRx(rxpacket, sts_id, self.data_length)
+                if result != COMM_SUCCESS:
+                    self.last_result = False
+                # print(sts_id)
+        else:
+            self.last_result = False
+        # print(self.last_result)
+        return result
+
+    def txRxPacket(self):
+        result = self.txPacket()
+        if result != COMM_SUCCESS:
+            return result
+
+        return self.rxPacket()
+
+    def readRx(self, rxpacket, sts_id, data_length):
+        # print(sts_id)
+        # print(rxpacket)
+        data = []
+        rx_length = len(rxpacket)
+        # print(rx_length)
+        rx_index = 0;
+        while (rx_index+6+data_length) <= rx_length:
+            headpacket = [0x00, 0x00, 0x00]
+            while rx_index < rx_length:
+                headpacket[2] = headpacket[1];
+                headpacket[1] = headpacket[0];
+                headpacket[0] = rxpacket[rx_index];
+                rx_index += 1
+                if (headpacket[2] == 0xFF) and (headpacket[1] == 0xFF) and headpacket[0] == sts_id:
+                    # print(rx_index)
+                    break
+            # print(rx_index+3+data_length)
+            if (rx_index+3+data_length) > rx_length:
+                break;
+            if rxpacket[rx_index] != (data_length+2):
+                rx_index += 1
+                # print(rx_index)
+                continue
+            rx_index += 1
+            Error = rxpacket[rx_index]
+            rx_index += 1
+            calSum = sts_id + (data_length+2) + Error
+            data = [Error]
+            data.extend(rxpacket[rx_index : rx_index+data_length])
+            for i in range(0, data_length):
+                calSum += rxpacket[rx_index]
+                rx_index += 1
+            calSum = ~calSum & 0xFF
+            # print(calSum)
+            if calSum != rxpacket[rx_index]:
+                return None, COMM_RX_CORRUPT
+            return data, COMM_SUCCESS 
+        # print(rx_index)
+        return None, COMM_RX_CORRUPT
+
+    def isAvailable(self, sts_id, address, data_length):
+        #if self.last_result is False or sts_id not in self.data_dict:
+        if sts_id not in self.data_dict:
+            return False, 0
+
+        if (address < self.start_address) or (self.start_address + self.data_length - data_length < address):
+            return False, 0
+        if not self.data_dict[sts_id]:
+            return False, 0
+        if len(self.data_dict[sts_id])<(data_length+1):
+            return False, 0
+        return True, self.data_dict[sts_id][0]
+
+    def getData(self, sts_id, address, data_length):
+        if data_length == 1:
+            return self.data_dict[sts_id][address-self.start_address+1]
+        elif data_length == 2:
+            return self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+1],
+                                self.data_dict[sts_id][address-self.start_address+2])
+        elif data_length == 4:
+            return self.ph.scs_makedword(self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+1],
+                                              self.data_dict[sts_id][address-self.start_address+2]),
+                                 self.ph.scs_makeword(self.data_dict[sts_id][address-self.start_address+3],
+                                              self.data_dict[sts_id][address-self.start_address+4]))
+        else:
+            return 0

toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python
+
+from .stservo_def import *
+
+class GroupSyncWrite:
+    def __init__(self, ph, start_address, data_length):
+        self.ph = ph
+        self.start_address = start_address
+        self.data_length = data_length
+
+        self.is_param_changed = False
+        self.param = []
+        self.data_dict = {}
+
+        self.clearParam()
+
+    def makeParam(self):
+        if not self.data_dict:
+            return
+
+        self.param = []
+
+        for sts_id in self.data_dict:
+            if not self.data_dict[sts_id]:
+                return
+
+            self.param.append(sts_id)
+            self.param.extend(self.data_dict[sts_id])
+
+    def addParam(self, sts_id, data):
+        if sts_id in self.data_dict:  # sts_id already exist
+            return False
+
+        if len(data) > self.data_length:  # input data is longer than set
+            return False
+
+        self.data_dict[sts_id] = data
+
+        self.is_param_changed = True
+        return True
+
+    def removeParam(self, sts_id):
+        if sts_id not in self.data_dict:  # NOT exist
+            return
+
+        del self.data_dict[sts_id]
+
+        self.is_param_changed = True
+
+    def changeParam(self, sts_id, data):
+        if sts_id not in self.data_dict:  # NOT exist
+            return False
+
+        if len(data) > self.data_length:  # input data is longer than set
+            return False
+
+        self.data_dict[sts_id] = data
+
+        self.is_param_changed = True
+        return True
+
+    def clearParam(self):
+        self.data_dict.clear()
+
+    def txPacket(self):
+        if len(self.data_dict.keys()) == 0:
+            return COMM_NOT_AVAILABLE
+
+        if self.is_param_changed is True or not self.param:
+            self.makeParam()
+
+        return self.ph.syncWriteTxOnly(self.start_address, self.data_length, self.param,
+                                       len(self.data_dict.keys()) * (1 + self.data_length))

toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py

@@ -0,0 +1,115 @@
+#!/usr/bin/env python
+
+import time
+import serial
+import sys
+import platform
+
+DEFAULT_BAUDRATE = 1000000
+LATENCY_TIMER = 50 
+
+class PortHandler(object):
+    def __init__(self, port_name):
+        self.is_open = False
+        self.baudrate = DEFAULT_BAUDRATE
+        self.packet_start_time = 0.0
+        self.packet_timeout = 0.0
+        self.tx_time_per_byte = 0.0
+
+        self.is_using = False
+        self.port_name = port_name
+        self.ser = None
+
+    def openPort(self):
+        return self.setBaudRate(self.baudrate)
+
+    def closePort(self):
+        self.ser.close()
+        self.is_open = False
+
+    def clearPort(self):
+        self.ser.flush()
+
+    def setPortName(self, port_name):
+        self.port_name = port_name
+
+    def getPortName(self):
+        return self.port_name
+
+    def setBaudRate(self, baudrate):
+        baud = self.getCFlagBaud(baudrate)
+
+        if baud <= 0:
+            # self.setupPort(38400)
+            # self.baudrate = baudrate
+            return False  # TODO: setCustomBaudrate(baudrate)
+        else:
+            self.baudrate = baudrate
+            return self.setupPort(baud)
+
+    def getBaudRate(self):
+        return self.baudrate
+
+    def getBytesAvailable(self):
+        return self.ser.in_waiting
+
+    def readPort(self, length):
+        if (sys.version_info > (3, 0)):
+            return self.ser.read(length)
+        else:
+            return [ord(ch) for ch in self.ser.read(length)]
+
+    def writePort(self, packet):
+        return self.ser.write(packet)
+
+    def setPacketTimeout(self, packet_length):
+        self.packet_start_time = self.getCurrentTime()
+        self.packet_timeout = (self.tx_time_per_byte * packet_length) + (self.tx_time_per_byte * 3.0) + LATENCY_TIMER
+
+    def setPacketTimeoutMillis(self, msec):
+        self.packet_start_time = self.getCurrentTime()
+        self.packet_timeout = msec
+
+    def isPacketTimeout(self):
+        if self.getTimeSinceStart() > self.packet_timeout:
+            self.packet_timeout = 0
+            return True
+
+        return False
+
+    def getCurrentTime(self):
+        return round(time.time() * 1000000000) / 1000000.0
+
+    def getTimeSinceStart(self):
+        time_since = self.getCurrentTime() - self.packet_start_time
+        if time_since < 0.0:
+            self.packet_start_time = self.getCurrentTime()
+
+        return time_since
+
+    def setupPort(self, cflag_baud):
+        if self.is_open:
+            self.closePort()
+
+        self.ser = serial.Serial(
+            port=self.port_name,
+            baudrate=self.baudrate,
+            # parity = serial.PARITY_ODD,
+            # stopbits = serial.STOPBITS_TWO,
+            bytesize=serial.EIGHTBITS,
+            timeout=0
+        )
+
+        self.is_open = True
+
+        self.ser.reset_input_buffer()
+
+        self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0
+
+        return True
+
+    def getCFlagBaud(self, baudrate):
+        if baudrate in [4800, 9600, 14400, 19200, 38400, 57600, 115200, 128000, 250000, 500000, 1000000]:
+            return baudrate
+        else:
+            return -1          

toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py

@@ -0,0 +1,530 @@
+#!/usr/bin/env python
+
+from .stservo_def import *
+
+TXPACKET_MAX_LEN = 250
+RXPACKET_MAX_LEN = 250
+
+# for Protocol Packet
+PKT_HEADER0 = 0
+PKT_HEADER1 = 1
+PKT_ID = 2
+PKT_LENGTH = 3
+PKT_INSTRUCTION = 4
+PKT_ERROR = 4
+PKT_PARAMETER0 = 5
+
+# Protocol Error bit
+ERRBIT_VOLTAGE = 1
+ERRBIT_ANGLE = 2
+ERRBIT_OVERHEAT = 4
+ERRBIT_OVERELE = 8
+ERRBIT_OVERLOAD = 32
+
+
+class protocol_packet_handler(object):
+    def __init__(self, portHandler, protocol_end):
+        #self.sts_setend(protocol_end)# STServo bit end(STS/SMS=0, SCS=1)
+        self.portHandler = portHandler
+        self.sts_end = protocol_end
+
+    def sts_getend(self):
+        return self.sts_end
+
+    def sts_setend(self, e):
+        self.sts_end = e
+
+    def sts_tohost(self, a, b):
+        if (a & (1<<b)):
+            return -(a & ~(1<<b))
+        else:
+            return a
+
+    def sts_toscs(self, a, b):
+        if (a<0):
+            return (-a | (1<<b))
+        else:
+            return a
+
+    def sts_makeword(self, a, b):
+        if self.sts_end==0:
+            return (a & 0xFF) | ((b & 0xFF) << 8)
+        else:
+            return (b & 0xFF) | ((a & 0xFF) << 8)
+
+    def sts_makedword(self, a, b):
+        return (a & 0xFFFF) | (b & 0xFFFF) << 16
+
+    def sts_loword(self, l):
+        return l & 0xFFFF
+
+    def sts_hiword(self, h):
+        return (h >> 16) & 0xFFFF
+
+    def sts_lobyte(self, w):
+        if self.sts_end==0:
+            return w & 0xFF
+        else:
+            return (w >> 8) & 0xFF
+
+    def sts_hibyte(self, w):
+        if self.sts_end==0:
+            return (w >> 8) & 0xFF
+        else:
+            return w & 0xFF
+        
+    def getProtocolVersion(self):
+        return 1.0
+
+    def getTxRxResult(self, result):
+        if result == COMM_SUCCESS:
+            return "[TxRxResult] Communication success!"
+        elif result == COMM_PORT_BUSY:
+            return "[TxRxResult] Port is in use!"
+        elif result == COMM_TX_FAIL:
+            return "[TxRxResult] Failed transmit instruction packet!"
+        elif result == COMM_RX_FAIL:
+            return "[TxRxResult] Failed get status packet from device!"
+        elif result == COMM_TX_ERROR:
+            return "[TxRxResult] Incorrect instruction packet!"
+        elif result == COMM_RX_WAITING:
+            return "[TxRxResult] Now receiving status packet!"
+        elif result == COMM_RX_TIMEOUT:
+            return "[TxRxResult] There is no status packet!"
+        elif result == COMM_RX_CORRUPT:
+            return "[TxRxResult] Incorrect status packet!"
+        elif result == COMM_NOT_AVAILABLE:
+            return "[TxRxResult] Protocol does not support this function!"
+        else:
+            return ""
+
+    def getRxPacketError(self, error):
+        if error & ERRBIT_VOLTAGE:
+            return "[ServoStatus] Input voltage error!"
+
+        if error & ERRBIT_ANGLE:
+            return "[ServoStatus] Angle sen error!"
+
+        if error & ERRBIT_OVERHEAT:
+            return "[ServoStatus] Overheat error!"
+
+        if error & ERRBIT_OVERELE:
+            return "[ServoStatus] OverEle error!"
+        
+        if error & ERRBIT_OVERLOAD:
+            return "[ServoStatus] Overload error!"
+
+        return ""
+
+    def txPacket(self, txpacket):
+        checksum = 0
+        total_packet_length = txpacket[PKT_LENGTH] + 4  # 4: HEADER0 HEADER1 ID LENGTH
+
+        if self.portHandler.is_using:
+            return COMM_PORT_BUSY
+        self.portHandler.is_using = True
+
+        # check max packet length
+        if total_packet_length > TXPACKET_MAX_LEN:
+            self.portHandler.is_using = False
+            return COMM_TX_ERROR
+
+        # make packet header
+        txpacket[PKT_HEADER0] = 0xFF
+        txpacket[PKT_HEADER1] = 0xFF
+
+        # add a checksum to the packet
+        for idx in range(2, total_packet_length - 1):  # except header, checksum
+            checksum += txpacket[idx]
+
+        txpacket[total_packet_length - 1] = ~checksum & 0xFF
+
+        #print "[TxPacket] %r" % txpacket
+
+        # tx packet
+        self.portHandler.clearPort()
+        written_packet_length = self.portHandler.writePort(txpacket)
+        if total_packet_length != written_packet_length:
+            self.portHandler.is_using = False
+            return COMM_TX_FAIL
+
+        return COMM_SUCCESS
+
+    def rxPacket(self):
+        rxpacket = []
+
+        result = COMM_TX_FAIL
+        checksum = 0
+        rx_length = 0
+        wait_length = 6  # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM)
+
+        while True:
+            rxpacket.extend(self.portHandler.readPort(wait_length - rx_length))
+            rx_length = len(rxpacket)
+            if rx_length >= wait_length:
+                # find packet header
+                for idx in range(0, (rx_length - 1)):
+                    if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF):
+                        break
+
+                if idx == 0:  # found at the beginning of the packet
+                    if (rxpacket[PKT_ID] > 0xFD) or (rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) or (
+                            rxpacket[PKT_ERROR] > 0x7F):
+                        # unavailable ID or unavailable Length or unavailable Error
+                        # remove the first byte in the packet
+                        del rxpacket[0]
+                        rx_length -= 1
+                        continue
+
+                    # re-calculate the exact length of the rx packet
+                    if wait_length != (rxpacket[PKT_LENGTH] + PKT_LENGTH + 1):
+                        wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1
+                        continue
+
+                    if rx_length < wait_length:
+                        # check timeout
+                        if self.portHandler.isPacketTimeout():
+                            if rx_length == 0:
+                                result = COMM_RX_TIMEOUT
+                            else:
+                                result = COMM_RX_CORRUPT
+                            break
+                        else:
+                            continue
+
+                    # calculate checksum
+                    for i in range(2, wait_length - 1):  # except header, checksum
+                        checksum += rxpacket[i]
+                    checksum = ~checksum & 0xFF
+
+                    # verify checksum
+                    if rxpacket[wait_length - 1] == checksum:
+                        result = COMM_SUCCESS
+                    else:
+                        result = COMM_RX_CORRUPT
+                    break
+
+                else:
+                    # remove unnecessary packets
+                    del rxpacket[0: idx]
+                    rx_length -= idx
+
+            else:
+                # check timeout
+                if self.portHandler.isPacketTimeout():
+                    if rx_length == 0:
+                        result = COMM_RX_TIMEOUT
+                    else:
+                        result = COMM_RX_CORRUPT
+                    break
+
+        self.portHandler.is_using = False
+        return rxpacket, result
+
+    def txRxPacket(self, txpacket):
+        rxpacket = None
+        error = 0
+
+        # tx packet
+        result = self.txPacket(txpacket)
+        if result != COMM_SUCCESS:
+            return rxpacket, result, error
+
+        # (ID == Broadcast ID) == no need to wait for status packet or not available
+        if (txpacket[PKT_ID] == BROADCAST_ID):
+            self.portHandler.is_using = False
+            return rxpacket, result, error
+
+        # set packet timeout
+        if txpacket[PKT_INSTRUCTION] == INST_READ:
+            self.portHandler.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6)
+        else:
+            self.portHandler.setPacketTimeout(6)  # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM
+
+        # rx packet
+        while True:
+            rxpacket, result = self.rxPacket()
+            if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]:
+                break
+
+        if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]:
+            error = rxpacket[PKT_ERROR]
+
+        return rxpacket, result, error
+
+    def ping(self, sts_id):
+        model_number = 0
+        error = 0
+
+        txpacket = [0] * 6
+
+        if sts_id >= BROADCAST_ID:
+            return model_number, COMM_NOT_AVAILABLE, error
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = 2
+        txpacket[PKT_INSTRUCTION] = INST_PING
+
+        rxpacket, result, error = self.txRxPacket(txpacket)
+
+        if result == COMM_SUCCESS:
+            data_read, result, error = self.readTxRx(sts_id, 3, 2)  # Address 3 : Model Number
+            if result == COMM_SUCCESS:
+                model_number = self.sts_makeword(data_read[0], data_read[1])
+
+        return model_number, result, error
+
+    def action(self, sts_id):
+        txpacket = [0] * 6
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = 2
+        txpacket[PKT_INSTRUCTION] = INST_ACTION
+
+        _, result, _ = self.txRxPacket(txpacket)
+
+        return result
+
+    def readTx(self, sts_id, address, length):
+
+        txpacket = [0] * 8
+
+        if sts_id >= BROADCAST_ID:
+            return COMM_NOT_AVAILABLE
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = 4
+        txpacket[PKT_INSTRUCTION] = INST_READ
+        txpacket[PKT_PARAMETER0 + 0] = address
+        txpacket[PKT_PARAMETER0 + 1] = length
+
+        result = self.txPacket(txpacket)
+
+        # set packet timeout
+        if result == COMM_SUCCESS:
+            self.portHandler.setPacketTimeout(length + 6)
+
+        return result
+
+    def readRx(self, sts_id, length):
+        result = COMM_TX_FAIL
+        error = 0
+
+        rxpacket = None
+        data = []
+
+        while True:
+            rxpacket, result = self.rxPacket()
+
+            if result != COMM_SUCCESS or rxpacket[PKT_ID] == sts_id:
+                break
+
+        if result == COMM_SUCCESS and rxpacket[PKT_ID] == sts_id:
+            error = rxpacket[PKT_ERROR]
+
+            data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0+length])
+
+        return data, result, error
+
+    def readTxRx(self, sts_id, address, length):
+        txpacket = [0] * 8
+        data = []
+
+        if sts_id >= BROADCAST_ID:
+            return data, COMM_NOT_AVAILABLE, 0
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = 4
+        txpacket[PKT_INSTRUCTION] = INST_READ
+        txpacket[PKT_PARAMETER0 + 0] = address
+        txpacket[PKT_PARAMETER0 + 1] = length
+
+        rxpacket, result, error = self.txRxPacket(txpacket)
+        if result == COMM_SUCCESS:
+            error = rxpacket[PKT_ERROR]
+
+            data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0+length])
+
+        return data, result, error
+
+    def read1ByteTx(self, sts_id, address):
+        return self.readTx(sts_id, address, 1)
+
+    def read1ByteRx(self, sts_id):
+        data, result, error = self.readRx(sts_id, 1)
+        data_read = data[0] if (result == COMM_SUCCESS) else 0
+        return data_read, result, error
+
+    def read1ByteTxRx(self, sts_id, address):
+        data, result, error = self.readTxRx(sts_id, address, 1)
+        data_read = data[0] if (result == COMM_SUCCESS) else 0
+        return data_read, result, error
+
+    def read2ByteTx(self, sts_id, address):
+        return self.readTx(sts_id, address, 2)
+
+    def read2ByteRx(self, sts_id):
+        data, result, error = self.readRx(sts_id, 2)
+        data_read = self.sts_makeword(data[0], data[1]) if (result == COMM_SUCCESS) else 0
+        return data_read, result, error
+
+    def read2ByteTxRx(self, sts_id, address):
+        data, result, error = self.readTxRx(sts_id, address, 2)
+        data_read = self.sts_makeword(data[0], data[1]) if (result == COMM_SUCCESS) else 0
+        return data_read, result, error
+
+    def read4ByteTx(self, sts_id, address):
+        return self.readTx(sts_id, address, 4)
+
+    def read4ByteRx(self, sts_id):
+        data, result, error = self.readRx(sts_id, 4)
+        data_read = self.sts_makedword(self.sts_makeword(data[0], data[1]),
+                                  self.sts_makeword(data[2], data[3])) if (result == COMM_SUCCESS) else 0
+        return data_read, result, error
+
+    def read4ByteTxRx(self, sts_id, address):
+        data, result, error = self.readTxRx(sts_id, address, 4)
+        data_read = self.sts_makedword(self.sts_makeword(data[0], data[1]),
+                                  self.sts_makeword(data[2], data[3])) if (result == COMM_SUCCESS) else 0
+        return data_read, result, error
+
+    def writeTxOnly(self, sts_id, address, length, data):
+        txpacket = [0] * (length + 7)
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = length + 3
+        txpacket[PKT_INSTRUCTION] = INST_WRITE
+        txpacket[PKT_PARAMETER0] = address
+
+        txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
+
+        result = self.txPacket(txpacket)
+        self.portHandler.is_using = False
+
+        return result
+
+    def writeTxRx(self, sts_id, address, length, data):
+        txpacket = [0] * (length + 7)
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = length + 3
+        txpacket[PKT_INSTRUCTION] = INST_WRITE
+        txpacket[PKT_PARAMETER0] = address
+
+        txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
+        rxpacket, result, error = self.txRxPacket(txpacket)
+
+        return result, error
+
+    def write1ByteTxOnly(self, sts_id, address, data):
+        data_write = [data]
+        return self.writeTxOnly(sts_id, address, 1, data_write)
+
+    def write1ByteTxRx(self, sts_id, address, data):
+        data_write = [data]
+        return self.writeTxRx(sts_id, address, 1, data_write)
+
+    def write2ByteTxOnly(self, sts_id, address, data):
+        data_write = [self.sts_lobyte(data), self.sts_hibyte(data)]
+        return self.writeTxOnly(sts_id, address, 2, data_write)
+
+    def write2ByteTxRx(self, sts_id, address, data):
+        data_write = [self.sts_lobyte(data), self.sts_hibyte(data)]
+        return self.writeTxRx(sts_id, address, 2, data_write)
+
+    def write4ByteTxOnly(self, sts_id, address, data):
+        data_write = [self.sts_lobyte(self.sts_loword(data)),
+                      self.sts_hibyte(self.sts_loword(data)),
+                      self.sts_lobyte(self.sts_hiword(data)),
+                      self.sts_hibyte(self.sts_hiword(data))]
+        return self.writeTxOnly(sts_id, address, 4, data_write)
+
+    def write4ByteTxRx(self, sts_id, address, data):
+        data_write = [self.sts_lobyte(self.sts_loword(data)),
+                      self.sts_hibyte(self.sts_loword(data)),
+                      self.sts_lobyte(self.sts_hiword(data)),
+                      self.sts_hibyte(self.sts_hiword(data))]
+        return self.writeTxRx(sts_id, address, 4, data_write)
+
+    def regWriteTxOnly(self, sts_id, address, length, data):
+        txpacket = [0] * (length + 7)
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = length + 3
+        txpacket[PKT_INSTRUCTION] = INST_REG_WRITE
+        txpacket[PKT_PARAMETER0] = address
+
+        txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
+
+        result = self.txPacket(txpacket)
+        self.portHandler.is_using = False
+
+        return result
+
+    def regWriteTxRx(self, sts_id, address, length, data):
+        txpacket = [0] * (length + 7)
+
+        txpacket[PKT_ID] = sts_id
+        txpacket[PKT_LENGTH] = length + 3
+        txpacket[PKT_INSTRUCTION] = INST_REG_WRITE
+        txpacket[PKT_PARAMETER0] = address
+
+        txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
+
+        _, result, error = self.txRxPacket(txpacket)
+
+        return result, error
+
+    def syncReadTx(self, start_address, data_length, param, param_length):
+        txpacket = [0] * (param_length + 8)
+        # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN CHKSUM
+
+        txpacket[PKT_ID] = BROADCAST_ID
+        txpacket[PKT_LENGTH] = param_length + 4  # 7: INST START_ADDR DATA_LEN CHKSUM
+        txpacket[PKT_INSTRUCTION] = INST_SYNC_READ
+        txpacket[PKT_PARAMETER0 + 0] = start_address
+        txpacket[PKT_PARAMETER0 + 1] = data_length
+
+        txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length]
+
+        # print(txpacket)
+        result = self.txPacket(txpacket)
+        return result
+
+    def syncReadRx(self, data_length, param_length):
+        wait_length = (6 + data_length) * param_length
+        self.portHandler.setPacketTimeout(wait_length)
+        rxpacket = []
+        rx_length = 0
+        while True:
+            rxpacket.extend(self.portHandler.readPort(wait_length - rx_length))
+            rx_length = len(rxpacket)
+            if rx_length >= wait_length:
+                result = COMM_SUCCESS
+                break
+            else:
+                # check timeout
+                if self.portHandler.isPacketTimeout():
+                    if rx_length == 0:
+                        result = COMM_RX_TIMEOUT
+                    else:
+                        result = COMM_RX_CORRUPT
+                    break
+        self.portHandler.is_using = False
+        return result, rxpacket
+
+    def syncWriteTxOnly(self, start_address, data_length, param, param_length):
+        txpacket = [0] * (param_length + 8)
+        # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM
+
+        txpacket[PKT_ID] = BROADCAST_ID
+        txpacket[PKT_LENGTH] = param_length + 4  # 4: INST START_ADDR DATA_LEN ... CHKSUM
+        txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE
+        txpacket[PKT_PARAMETER0 + 0] = start_address
+        txpacket[PKT_PARAMETER0 + 1] = data_length
+
+        txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length]
+
+        _, result, _ = self.txRxPacket(txpacket)
+
+        return result

toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python
+
+from .stservo_def import *
+from .protocol_packet_handler import *
+from .group_sync_write import *
+
+#波特率定义
+SCSCL_1M = 0
+SCSCL_0_5M = 1
+SCSCL_250K = 2
+SCSCL_128K = 3
+SCSCL_115200 = 4
+SCSCL_76800 = 5
+SCSCL_57600 = 6
+SCSCL_38400 = 7
+
+#内存表定义
+#-------EPROM(只读)--------
+SCSCL_MODEL_L = 3
+SCSCL_MODEL_H = 4
+
+#-------EPROM(读写)--------
+scs_id = 5
+SCSCL_BAUD_RATE = 6
+SCSCL_MIN_ANGLE_LIMIT_L = 9
+SCSCL_MIN_ANGLE_LIMIT_H = 10
+SCSCL_MAX_ANGLE_LIMIT_L = 11
+SCSCL_MAX_ANGLE_LIMIT_H = 12
+SCSCL_CW_DEAD = 26
+SCSCL_CCW_DEAD = 27
+
+#-------SRAM(读写)--------
+SCSCL_TORQUE_ENABLE = 40
+SCSCL_GOAL_POSITION_L = 42
+SCSCL_GOAL_POSITION_H = 43
+SCSCL_GOAL_TIME_L = 44
+SCSCL_GOAL_TIME_H = 45
+SCSCL_GOAL_SPEED_L = 46
+SCSCL_GOAL_SPEED_H = 47
+SCSCL_LOCK = 48
+
+#-------SRAM(只读)--------
+SCSCL_PRESENT_POSITION_L  = 56
+SCSCL_PRESENT_POSITION_H = 57
+SCSCL_PRESENT_SPEED_L = 58
+SCSCL_PRESENT_SPEED_H = 59
+SCSCL_PRESENT_LOAD_L = 60
+SCSCL_PRESENT_LOAD_H = 61
+SCSCL_PRESENT_VOLTAGE = 62
+SCSCL_PRESENT_TEMPERATURE = 63
+SCSCL_MOVING = 66
+SCSCL_PRESENT_CURRENT_L = 69
+SCSCL_PRESENT_CURRENT_H = 70
+
+class scscl(protocol_packet_handler):
+    def __init__(self, portHandler):
+        protocol_packet_handler.__init__(self, portHandler, 1)
+        self.groupSyncWrite = GroupSyncWrite(self, SCSCL_GOAL_POSITION_L, 6)
+
+    def WritePos(self, scs_id, position, time, speed):
+        txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)]
+        return self.writeTxRx(scs_id, SCSCL_GOAL_POSITION_L, len(txpacket), txpacket)
+
+    def ReadPos(self, scs_id):
+        scs_present_position, scs_comm_result, scs_error = self.read2ByteTxRx(scs_id, SCSCL_PRESENT_POSITION_L)
+        return scs_present_position, scs_comm_result, scs_error
+
+    def ReadSpeed(self, scs_id):
+        scs_present_speed, scs_comm_result, scs_error = self.read2ByteTxRx(scs_id, SCSCL_PRESENT_SPEED_L)
+        return self.scs_tohost(scs_present_speed, 15), scs_comm_result, scs_error
+
+    def ReadPosSpeed(self, scs_id):
+        scs_present_position_speed, scs_comm_result, scs_error = self.read4ByteTxRx(scs_id, SCSCL_PRESENT_POSITION_L)
+        scs_present_position = self.scs_loword(scs_present_position_speed)
+        scs_present_speed = self.scs_hiword(scs_present_position_speed)
+        return scs_present_position, self.scs_tohost(scs_present_speed, 15), scs_comm_result, scs_error
+
+    def ReadMoving(self, scs_id):
+        moving, scs_comm_result, scs_error = self.read1ByteTxRx(scs_id, SCSCL_MOVING)
+        return moving, scs_comm_result, scs_error
+
+    def SyncWritePos(self, scs_id, position, time, speed):
+        txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)]
+        return self.groupSyncWrite.addParam(scs_id, txpacket)
+
+    def RegWritePos(self, scs_id, position, time, speed):
+        txpacket = [self.scs_lobyte(position), self.scs_hibyte(position), self.scs_lobyte(time), self.scs_hibyte(time), self.scs_lobyte(speed), self.scs_hibyte(speed)]
+        return self.regWriteTxRx(scs_id, SCSCL_GOAL_POSITION_L, len(txpacket), txpacket)
+
+    def RegAction(self):
+        return self.action(BROADCAST_ID)
+
+    def PWMMode(self, scs_id):
+        txpacket = [0, 0, 0, 0]
+        return self.writeTxRx(scs_id, SCSCL_MIN_ANGLE_LIMIT_L, len(txpacket), txpacket)
+
+    def WritePWM(self, scs_id, time):
+        return self.write2ByteTxRx(scs_id, SCSCL_GOAL_TIME_L, self.scs_toscs(time, 10))
+
+    def LockEprom(self, scs_id):
+        return self.write1ByteTxRx(scs_id, SCSCL_LOCK, 1)
+
+    def unLockEprom(self, scs_id):
+        return self.write1ByteTxRx(scs_id, SCSCL_LOCK, 0)

toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py

@@ -0,0 +1,7 @@
+from setuptools import setup, find_packages
+
+setup(
+    name='STservo_sdk',
+    version='0.1',
+    packages=find_packages(),
+)

toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py

@@ -0,0 +1,144 @@
+#!/usr/bin/env python
+
+from .stservo_def import *
+from .protocol_packet_handler import *
+from .group_sync_read import *
+from .group_sync_write import *
+
+#波特率定义
+STS_1M = 0
+STS_0_5M = 1
+STS_250K = 2
+STS_128K = 3
+STS_115200 = 4
+STS_76800 = 5
+STS_57600 = 6
+STS_38400 = 7
+
+#内存表定义
+#-------EPROM(只读)--------
+STS_MODEL_L = 3
+STS_MODEL_H = 4
+
+#-------EPROM(读写)--------
+STS_ID = 5
+STS_BAUD_RATE = 6
+STS_MIN_ANGLE_LIMIT_L = 9
+STS_MIN_ANGLE_LIMIT_H = 10
+STS_MAX_ANGLE_LIMIT_L = 11
+STS_MAX_ANGLE_LIMIT_H = 12
+STS_CW_DEAD = 26
+STS_CCW_DEAD = 27
+STS_OFS_L = 31
+STS_OFS_H = 32
+STS_MODE = 33
+
+#-------SRAM(读写)--------
+MAX_POSITION_L = 42
+STS_GOAL_POSITION_H = 43
+STS_GOAL_TIME_L = 44
+STS_GOAL_TIME_H = 45
+STS_LOAD_LIMIT = 36
+STS_TORQUE_ENABLE = 40
+STS_ACC = 41
+STS_GOAL_SPEED_L = 46
+STS_GOAL_SPEED_H = 47
+STS_MAX_LOAD = 0x30
+STS_LOCK = 55
+
+#-------SRAM(只读)--------
+STS_PRESENT_POSITION_L = 56
+STS_PRESENT_POSITION_H = 57
+STS_PRESENT_SPEED_L = 58
+STS_PRESENT_SPEED_H = 59
+STS_PRESENT_LOAD_L = 60
+STS_PRESENT_LOAD_H = 61
+STS_PRESENT_VOLTAGE = 62
+STS_PRESENT_TEMPERATURE = 63
+STS_MOVING = 66
+STS_PRESENT_CURRENT_L = 69
+STS_PRESENT_CURRENT_H = 70
+
+class sts(protocol_packet_handler):
+    def __init__(self, portHandler):
+        protocol_packet_handler.__init__(self, portHandler, 0)
+        self.groupSyncWrite = GroupSyncWrite(self, 40, 7)
+
+    def WritePosEx(self, sts_id, position, speed, acc):
+        txpacket = [acc, self.sts_lobyte(position), self.sts_hibyte(position), 0, 0, self.sts_lobyte(speed), self.sts_hibyte(speed)]
+        return self.writeTxRx(sts_id, STS_ACC, len(txpacket), txpacket)
+
+    def ReadPos(self, sts_id):
+        sts_present_position, sts_comm_result, sts_error = self.read2ByteTxRx(sts_id, 60)
+        return self.sts_tohost(sts_present_position, 15), sts_comm_result, sts_error
+    
+    def ReadLoad(self, sts_id):
+         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
+
+    def ReadSpeed(self, sts_id):
+        sts_present_speed, sts_comm_result, sts_error = self.read2ByteTxRx(sts_id, STS_PRESENT_SPEED_L)
+        return self.sts_tohost(sts_present_speed, 15), sts_comm_result, sts_error
+
+    def ReadPosSpeed(self, sts_id):
+        sts_present_position_speed, sts_comm_result, sts_error = self.read4ByteTxRx(sts_id, STS_PRESENT_POSITION_L)
+        sts_present_position = self.sts_loword(sts_present_position_speed)
+        sts_present_speed = self.sts_hiword(sts_present_position_speed)
+        return self.sts_tohost(sts_present_position, 15), self.sts_tohost(sts_present_speed, 15), sts_comm_result, sts_error
+
+    def ReadPosLoad(self, sts_id):
+        sts_present_position_speed, sts_comm_result, sts_error = self.read4ByteTxRx(sts_id, 60)
+        sts_present_position = self.sts_loword(sts_present_position_speed)
+        sts_present_speed = self.sts_hiword(sts_present_position_speed)
+        return self.sts_tohost(sts_present_position, 15), self.sts_tohost(sts_present_speed, 15), sts_comm_result, sts_error
+    
+    def ReadPosLoad1(self, sts_id):
+        sts_present_position_speed, sts_comm_result, sts_error = self.read4ByteTxRx(sts_id, 61)
+        sts_present_position = self.sts_loword(sts_present_position_speed)
+        sts_present_speed = self.sts_hiword(sts_present_position_speed)
+        return self.sts_tohost(sts_present_position, 15), self.sts_tohost(sts_present_speed, 15), sts_comm_result, sts_error
+
+    def ReadMoving(self, sts_id):
+        moving, sts_comm_result, sts_error = self.read1ByteTxRx(sts_id, STS_MOVING)
+        return moving, sts_comm_result, sts_error
+   
+    def MaxLoad(self, sts_id):
+        max_load, sts_comm_result, sts_error = self.read1ByteTxRx(sts_id, STS_LOAD_LIMIT)
+        return max_load, sts_comm_result, sts_error
+
+    def SyncWritePosEx(self, sts_id, position, speed, acc):
+        txpacket = [acc, self.sts_lobyte(position), self.sts_hibyte(position), 0, 0, self.sts_lobyte(speed), self.sts_hibyte(speed)]
+        return self.groupSyncWrite.addParam(sts_id, txpacket)
+
+    def RegWritePosEx(self, sts_id, position, speed, acc):
+        txpacket = [acc, self.sts_lobyte(position), self.sts_hibyte(position), 0, 0, self.sts_lobyte(speed), self.sts_hibyte(speed)]
+        return self.regWriteTxRx(sts_id, STS_ACC, len(txpacket), txpacket)
+
+    def RegAction(self):
+        return self.action(BROADCAST_ID)
+
+    def WheelMode(self, sts_id):
+        return self.write1ByteTxRx(sts_id, STS_MODE, 1)
+    
+    def SetMaxLoad(self, sts_id, load):
+        return self.write2ByteTxRx(sts_id, STS_MAX_LOAD, int(load*10))
+
+    def WriteSpec(self, sts_id, speed, acc):
+        speed = self.sts_toscs(speed, 15)
+        txpacket = [acc, 0, 0, 0, 0, self.sts_lobyte(speed), self.sts_hibyte(speed)]
+        return self.writeTxRx(sts_id, STS_ACC, len(txpacket), txpacket)
+
+    def LockEprom(self, sts_id):
+        return self.write1ByteTxRx(sts_id, STS_LOCK, 1)
+
+    def unLockEprom(self, sts_id):
+        return self.write1ByteTxRx(sts_id, STS_LOCK, 0)
+

toid_interaction/toid_interaction/mechanism/STservo_sdk/stservo_def.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+
+BROADCAST_ID = 0xFE  # 254
+MAX_ID = 0xFC  # 252
+STS_END = 0
+
+# Instruction for STS Protocol
+INST_PING = 1
+INST_READ = 2
+INST_WRITE = 3
+INST_REG_WRITE = 4
+INST_ACTION = 5
+INST_SYNC_WRITE = 131  # 0x83
+INST_SYNC_READ = 130  # 0x82
+
+# Communication Result
+COMM_SUCCESS = 0  # tx or rx packet communication success
+COMM_PORT_BUSY = -1  # Port is busy (in use)
+COMM_TX_FAIL = -2  # Failed transmit instruction packet
+COMM_RX_FAIL = -3  # Failed get status packet
+COMM_TX_ERROR = -4  # Incorrect instruction packet
+COMM_RX_WAITING = -5  # Now recieving status packet
+COMM_RX_TIMEOUT = -6  # There is no status packet
+COMM_RX_CORRUPT = -7  # Incorrect status packet
+COMM_NOT_AVAILABLE = -9  #

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

@@ -0,0 +1,67 @@
+# u ovom fajlu sam napisala celu sekvencu koju obuhvata hvatanje zirova podizanje, okretanje, spustanje i odlaganje
+# dole cu napisaci koje funkicje su koja sekvenca ono sto smo pricali , nisam htela da pravim jos jedan fajl sa sekvencama da
+# ne bude da ima opet previse fajlova
+# sekvecncu 4 nije jos zavrsila poslacu ti to naknadno
+from .zidovi_load import ZidoviAction
+from .zupcanik import ZupcanikAction
+import time
+import serial
+import serial.tools.list_ports as list_ports
+
+SERIAL_ID = "259B221729115453"
+
+
+def okreni(i):
+    for port_info in list_ports.comports():
+        if port_info.serial_number == SERIAL_ID:
+            break
+
+    ser = serial.Serial(port_info.device, 115200, timeout=1)
+    ser.write(str(i).encode())
+
+
+def okreni_niz(broj):
+    """
+    Funkcija koja prima string od 4 karaktera (0 ili 1)
+    """
+    if len(broj) != 4:
+        raise ValueError("Binarni niz mora imati tačno 4 karaktera")
+
+    okreni(6)
+    for i, char in enumerate(broj):
+        if char == "1":
+            okreni(i + 1)
+
+
+def main():
+
+    zidovi = ZidoviAction('/dev/ttyACM1')
+    zupcanik = ZupcanikAction('/dev/ttyACM1')
+
+
+    # sekvenca 1
+    okreni(5)
+    zupcanik.zupcanik(1, -1010, 25)
+
+    # sekvenca 2
+    zidovi.beli_zid(1)
+    zidovi.plavi_zid(1)
+
+    okreni_niz("1010")
+
+    zidovi.plavi_zid(0, TargetPos=150)
+    zidovi.beli_zid(0, TargetPos=450)
+
+
+    # sekvenca 3
+
+    zupcanik.zupcanik(1, 1010, 25)
+
+    zidovi.plavi_zid(0, TargetPos=150)
+    zidovi.beli_zid(0, TargetPos=150)
+
+    time.sleep(1)
+
+
+if __name__ == "__main__":
+    main()

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_interaction/toid_interaction/mechanism/zidovi_load.py

@@ -0,0 +1,366 @@
+from .STservo_sdk import PortHandler, sts, COMM_SUCCESS, STS_ID
+# da li ovo dole treba da importujem ako je nalazi u STservo_sdk?
+# from stservo_def import COMM_SUCCESS
+
+# Default settings
+
+
+class ZidoviAction:
+    BAUDRATE : int
+    DEVICENAME : str
+    STS_MOVING_ACC: int
+    def __init__(
+        self,
+        devicename,  # Check which port is being used on your controller
+        baudrate=1000000,  # STServo default baudrate
+        sts_moving_acc=0,  # STServo moving acceleration
+    ):
+        self.BAUDRATE = baudrate
+        self.DEVICENAME = devicename
+        self.STS_MOVING_ACC = sts_moving_acc
+
+
+    def plavi_zid(self, smer, brzina=1500, opterecenje_threshold=75, TargetPos=300):
+        # TargetPos = 300 # Target position in degrees
+        TargetPos = TargetPos * 4096 / 360  # Convert to motor ticks
+        if smer == 1:
+            Speed_ID2 = brzina  # Speed for motor ID 2
+            Speed_ID4 = -brzina  # Speed for motor ID 4 (opposite sign)
+        else:
+            Speed_ID2 = -brzina  # Speed for motor ID 2
+            Speed_ID4 = brzina  # Speed for motor ID 4 (opposite sign)
+
+        # Initialize PortHandler instance
+        portHandler = PortHandler(self.DEVICENAME)
+
+        # Initialize PacketHandler instance
+        packetHandler = sts(portHandler)
+
+        # Open port
+        if portHandler.openPort():
+            print("Succeeded to open the port")
+        else:
+            print("Failed to open the port")
+            print("Press any key to terminate...")
+
+            quit()
+
+        # Set port baudrate
+        if portHandler.setBaudRate(self.BAUDRATE):
+            print("Succeeded to change the baudrate")
+        else:
+            print("Failed to change the baudrate")
+            print("Press any key to terminate...")
+
+            quit()
+
+        # Set motors to Wheel Mode
+        for motor_id in [2, 4]:
+            sts_comm_result, sts_error = packetHandler.WheelMode(motor_id)
+            if sts_comm_result != COMM_SUCCESS:
+                print(
+                    "[ID:%01d] %s"
+                    % (motor_id, packetHandler.getTxRxResult(sts_comm_result))
+                )
+            elif sts_error != 0:
+                print(
+                    "[ID:%01d] %s"
+                    % (motor_id, packetHandler.getRxPacketError(sts_error))
+                )
+
+        # Read initial positions for both motors
+        sts_present_position_ID2, sts_present_speed_ID2, sts_comm_result, sts_error = (
+            packetHandler.ReadPosSpeed(2)
+        )
+        sts_present_position_ID4, sts_present_speed_ID4, sts_comm_result, sts_error = (
+            packetHandler.ReadPosSpeed(4)
+        )
+
+        print("Initial positions:")
+        print(
+            "[ID:002] PresPos:%d PresSpd:%d"
+            % (sts_present_position_ID2, sts_present_speed_ID2)
+        )
+        print(
+            "[ID:004] PresPos:%d PresSpd:%d"
+            % (sts_present_position_ID4, sts_present_speed_ID4)
+        )
+
+        # Initialize variables for tracking movement
+        TrenutnaPos_ID2 = sts_present_position_ID2
+        ProslaPos_ID2 = sts_present_position_ID2
+        PredjeniPut_ID2 = 0
+
+        TrenutnaPos_ID4 = sts_present_position_ID4
+        ProslaPos_ID4 = sts_present_position_ID4
+        PredjeniPut_ID4 = 0
+        packetHandler.SetMaxLoad(STS_ID, 80)
+
+        while True:
+            # Write goal position, speed, and acceleration for both motors
+            sts_comm_result, sts_error = packetHandler.WriteSpec(
+                2, Speed_ID2, self.STS_MOVING_ACC
+            )
+            if sts_comm_result != COMM_SUCCESS:
+                print("[ID:002] %s" % packetHandler.getTxRxResult(sts_comm_result))
+            if sts_error != 0:
+                print("[ID:002] %s" % packetHandler.getRxPacketError(sts_error))
+
+            sts_comm_result, sts_error = packetHandler.WriteSpec(
+                4, Speed_ID4, self.STS_MOVING_ACC
+            )
+            if sts_comm_result != COMM_SUCCESS:
+                print("[ID:004] %s" % packetHandler.getTxRxResult(sts_comm_result))
+            if sts_error != 0:
+                print("[ID:004] %s" % packetHandler.getRxPacketError(sts_error))
+
+            # Update positions for both motors
+            ProslaPos_ID2 = TrenutnaPos_ID2
+            ProslaPos_ID4 = TrenutnaPos_ID4
+
+            (
+                sts_present_position_ID2,
+                sts_present_speed_ID2,
+                sts_comm_result,
+                sts_error,
+            ) = packetHandler.ReadPosSpeed(2)
+            if sts_comm_result != COMM_SUCCESS:
+                print("[ID:002] %s" % packetHandler.getTxRxResult(sts_comm_result))
+            if sts_error != 0:
+                print("[ID:002] %s" % packetHandler.getRxPacketError(sts_error))
+
+            (
+                sts_present_position_ID4,
+                sts_present_speed_ID4,
+                sts_comm_result,
+                sts_error,
+            ) = packetHandler.ReadPosSpeed(4)
+            if sts_comm_result != COMM_SUCCESS:
+                print("[ID:004] %s" % packetHandler.getTxRxResult(sts_comm_result))
+            if sts_error != 0:
+                print("[ID:004] %s" % packetHandler.getRxPacketError(sts_error))
+            sts_present_load2, sts_comm_result, sts_error = packetHandler.ReadLoad(2)
+            sts_present_load4, sts_comm_result, sts_error = packetHandler.ReadLoad(4)
+            opterecenje2 = (sts_present_load2 & ((1 << 10) - 1)) * 0.1
+            opterecenje4 = (sts_present_load4 & ((1 << 10) - 1)) * 0.1
+            print("Current positions:")
+            print(
+                "[ID:002] PresPos:%d PresSpd:%d Load:%d"
+                % (sts_present_position_ID2, sts_present_speed_ID2, opterecenje2)
+            )
+            print(
+                "[ID:004] PresPos:%d PresSpd:%d Load:%d"
+                % (sts_present_position_ID4, sts_present_speed_ID4, opterecenje4)
+            )
+
+            TrenutnaPos_ID2 = sts_present_position_ID2
+            TrenutnaPos_ID4 = sts_present_position_ID4
+
+            # Check if the positions have changed
+            if (
+                opterecenje2 > opterecenje_threshold
+                or opterecenje4 > opterecenje_threshold
+            ):
+                print(
+                    "High load detected: [ID:002] PresPos:%d PresSpd:%d Load:%d"
+                    % (sts_present_position_ID2, sts_present_speed_ID2, opterecenje2)
+                )
+                print(
+                    "High load detected: [ID:004] PresPos:%d PresSpd:%d Load:%d"
+                    % (sts_present_position_ID4, sts_present_speed_ID4, opterecenje4)
+                )
+                packetHandler.WriteSpec(2, 0, 0)  # Stop motor ID 2
+                packetHandler.WriteSpec(4, 0, 0)  # Stop motor ID 4
+                break
+            if (
+                abs(TrenutnaPos_ID2 - ProslaPos_ID2) > 3000
+                or abs(TrenutnaPos_ID4 - ProslaPos_ID4) > 3000
+            ):
+                PredjeniPut_ID2 = PredjeniPut_ID2
+                PredjeniPut_ID4 = PredjeniPut_ID4
+            else:
+                PredjeniPut_ID2 += abs(TrenutnaPos_ID2 - ProslaPos_ID2)
+                PredjeniPut_ID4 += abs(TrenutnaPos_ID4 - ProslaPos_ID4)
+
+            print("PredjeniPut ID2:%d deg " % (PredjeniPut_ID2 * 360 / 4096))
+            print("PredjeniPut ID4:%d deg " % (PredjeniPut_ID4 * 360 / 4096))
+
+            # Stop if both motors reach their target positions
+            if PredjeniPut_ID2 >= TargetPos and PredjeniPut_ID4 >= TargetPos + 10:
+                print("Target positions reached.")
+                packetHandler.WriteSpec(2, 0, 0)  # Stop motor ID 2
+                packetHandler.WriteSpec(4, 0, 0)  # Stop motor ID 4
+                break
+
+        # Close port
+        portHandler.closePort()
+
+    def beli_zid(self, smer, brzina=1500, opterecenje_threshold=75, TargetPos=600):
+        # TargetPos = 600  # Target position in degrees
+        TargetPos = TargetPos * 4096 / 360  # Convert to motor ticks
+        if smer == 1:
+            Speed_ID3 = -brzina  # Speed for motor ID 3
+            Speed_ID5 = brzina  # Speed for motor ID 5 (opposite sign)
+
+        else:
+            Speed_ID3 = brzina  # Speed for motor ID 3
+            Speed_ID5 = -brzina  # Speed for motor ID 5 (opposite sign)
+
+        # Initialize PortHandler instance
+        portHandler = PortHandler(self.DEVICENAME)
+
+        # Initialize PacketHandler instance
+        packetHandler = sts(portHandler)
+
+        # Open port
+        if portHandler.openPort():
+            print("Succeeded to open the port")
+        else:
+            print("Failed to open the port")
+            print("Press any key to terminate...")
+
+            quit()
+
+        # Set port baudrate
+        if portHandler.setBaudRate(self.BAUDRATE):
+            print("Succeeded to change the baudrate")
+        else:
+            print("Failed to change the baudrate")
+            print("Press any key to terminate...")
+
+            quit()
+
+        # Set motors to Wheel Mode
+        for motor_id in [3, 5]:
+            sts_comm_result, sts_error = packetHandler.WheelMode(motor_id)
+            if sts_comm_result != COMM_SUCCESS:
+                print(
+                    "[ID:%01d] %s"
+                    % (motor_id, packetHandler.getTxRxResult(sts_comm_result))
+                )
+            elif sts_error != 0:
+                print(
+                    "[ID:%01d] %s"
+                    % (motor_id, packetHandler.getRxPacketError(sts_error))
+                )
+
+        # Read initial positions for both motors
+        sts_present_position_ID3, sts_present_speed_ID3, sts_comm_result, sts_error = (
+            packetHandler.ReadPosSpeed(3)
+        )
+        sts_present_position_ID5, sts_present_speed_ID5, sts_comm_result, sts_error = (
+            packetHandler.ReadPosSpeed(5)
+        )
+
+        print("Initial positions:")
+        #  print("[ID:003] PresPos:%d PresSpd:%d" % (sts_present_position_ID3, sts_present_speed_ID3))
+        # print("[ID:005] PresPos:%d PresSpd:%d" % (sts_present_position_ID5, sts_present_speed_ID5))
+
+        # Initialize variables for tracking movement
+        TrenutnaPos_ID3 = sts_present_position_ID3
+        ProslaPos_ID3 = sts_present_position_ID3
+        PredjeniPut_ID3 = 0
+
+        TrenutnaPos_ID5 = sts_present_position_ID5
+        ProslaPos_ID5 = sts_present_position_ID5
+        PredjeniPut_ID5 = 0
+        packetHandler.SetMaxLoad(STS_ID, 80)
+
+        while True:
+            # Write goal position, speed, and acceleration for both motors
+            sts_comm_result, sts_error = packetHandler.WriteSpec(
+                3, Speed_ID3, self.STS_MOVING_ACC
+            )
+            if sts_comm_result != COMM_SUCCESS:
+                print("[ID:003] %s" % packetHandler.getTxRxResult(sts_comm_result))
+            if sts_error != 0:
+                print("[ID:003] %s" % packetHandler.getRxPacketError(sts_error))
+
+            sts_comm_result, sts_error = packetHandler.WriteSpec(
+                5, Speed_ID5, self.STS_MOVING_ACC
+            )
+            if sts_comm_result != COMM_SUCCESS:
+                print("[ID:005] %s" % packetHandler.getTxRxResult(sts_comm_result))
+            if sts_error != 0:
+                print("[ID:005] %s" % packetHandler.getRxPacketError(sts_error))
+
+            # Update positions for both motors
+            ProslaPos_ID3 = TrenutnaPos_ID3
+            ProslaPos_ID5 = TrenutnaPos_ID5
+
+            (
+                sts_present_position_ID3,
+                sts_present_speed_ID3,
+                sts_comm_result,
+                sts_error,
+            ) = packetHandler.ReadPosSpeed(3)
+            if sts_comm_result != COMM_SUCCESS:
+                print("[ID:003] %s" % packetHandler.getTxRxResult(sts_comm_result))
+            if sts_error != 0:
+                print("[ID:003] %s" % packetHandler.getRxPacketError(sts_error))
+
+            (
+                sts_present_position_ID5,
+                sts_present_speed_ID5,
+                sts_comm_result,
+                sts_error,
+            ) = packetHandler.ReadPosSpeed(5)
+            if sts_comm_result != COMM_SUCCESS:
+                print("[ID:005] %s" % packetHandler.getTxRxResult(sts_comm_result))
+            if sts_error != 0:
+                print("[ID:005] %s" % packetHandler.getRxPacketError(sts_error))
+            sts_present_load3, sts_comm_result, sts_error = packetHandler.ReadLoad(3)
+            sts_present_load5, sts_comm_result, sts_error = packetHandler.ReadLoad(5)
+            opterecenje3 = (sts_present_load3 & ((1 << 10) - 1)) * 0.1
+            opterecenje5 = (sts_present_load5 & ((1 << 10) - 1)) * 0.1
+            print("Current positions:")
+            print(
+                "[ID:003] PresPos:%d PresSpd:%d Load:%d"
+                % (sts_present_position_ID3, sts_present_speed_ID3, opterecenje3)
+            )
+            print(
+                "[ID:005] PresPos:%d PresSpd:%d Load:%d"
+                % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5)
+            )
+
+            TrenutnaPos_ID3 = sts_present_position_ID3
+            TrenutnaPos_ID5 = sts_present_position_ID5
+
+            # Check if the positions have changed
+            if (
+                opterecenje3 > opterecenje_threshold
+                or opterecenje5 > opterecenje_threshold
+            ):
+                print(
+                    "High load detected: [ID:003] PresPos:%d PresSpd:%d Load:%d"
+                    % (sts_present_position_ID3, sts_present_speed_ID3, opterecenje3)
+                )
+                print(
+                    "High load detected: [ID:005] PresPos:%d PresSpd:%d Load:%d"
+                    % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5)
+                )
+                packetHandler.WriteSpec(3, 0, 0)  # Stop motor ID 3
+                packetHandler.WriteSpec(5, 0, 0)  # Stop motor ID 5
+                break
+            if (
+                abs(TrenutnaPos_ID3 - ProslaPos_ID3) > 3000
+                or abs(TrenutnaPos_ID5 - ProslaPos_ID5) > 3000
+            ):
+                PredjeniPut_ID3 = PredjeniPut_ID3
+                PredjeniPut_ID5 = PredjeniPut_ID5
+            else:
+                PredjeniPut_ID3 += abs(TrenutnaPos_ID3 - ProslaPos_ID3)
+                PredjeniPut_ID5 += abs(TrenutnaPos_ID5 - ProslaPos_ID5)
+
+            print("PredjeniPut ID3:%d deg " % (PredjeniPut_ID3 * 360 / 4096))
+            print("PredjeniPut ID5:%d deg " % (PredjeniPut_ID5 * 360 / 4096))
+
+            # Stop if both motors reach their target positions
+            if PredjeniPut_ID3 >= TargetPos and PredjeniPut_ID5 >= TargetPos + 10:
+                print("Target positions reached.")
+                packetHandler.WriteSpec(3, 0, 0)  # Stop motor ID 3
+                packetHandler.WriteSpec(5, 0, 0)  # Stop motor ID 5
+                break
+
+        # Close port
+        portHandler.closePort()

toid_interaction/toid_interaction/mechanism/zupcanik.py

@@ -0,0 +1,130 @@
+from .STservo_sdk import PortHandler, sts, COMM_SUCCESS
+
+class ZupcanikAction:
+    BAUDRATE: int
+    DEVICENAME: str
+    STS_MOVING_ACC: int
+
+    def __init__(self, devicename, baudrate=1000000, sts_moving_acc=50):
+        self.BAUDRATE = baudrate
+        self.DEVICENAME = devicename
+        self.STS_MOVING_ACC = sts_moving_acc
+
+    def zupcanik(self, STS_ID, STS_MOVING_SPEED, TargetPos):
+
+        # Initialize PortHandler instance
+        portHandler = PortHandler(self.DEVICENAME)
+
+        # Initialize PacketHandler instance
+        packetHandler = sts(portHandler)
+
+        # Open port
+        if not portHandler.openPort():
+            print("Failed to open the port")
+            return
+
+        # Set port baudrate
+        if not portHandler.setBaudRate(self.BAUDRATE):
+            print("Failed to change the baudrate")
+            return
+
+        sts_comm_result, sts_error = packetHandler.WheelMode(STS_ID)
+        if sts_comm_result != COMM_SUCCESS:
+            print("%s" % packetHandler.getTxRxResult(sts_comm_result))
+            return
+        elif sts_error != 0:
+            print("%s" % packetHandler.getRxPacketError(sts_error))
+            return
+        # Read STServo present position
+        sts_present_position, sts_present_speed, sts_comm_result, sts_error = (
+            packetHandler.ReadPosSpeed(STS_ID)
+        )
+        if sts_comm_result != COMM_SUCCESS:
+            print(packetHandler.getTxRxResult(sts_comm_result))
+        else:
+            print(
+                "[ID:%03d] PresPos:%d PresSpd:%d"
+                % (STS_ID, sts_present_position, sts_present_speed)
+            )
+        if sts_error != 0:
+            print(packetHandler.getRxPacketError(sts_error))
+
+        TargetPos = TargetPos * 4096 / 360  # Convert to motor ticks
+        sts_present_position, sts_present_speed, sts_comm_result, sts_error = (
+            packetHandler.ReadPosSpeed(STS_ID)
+        )
+
+        print(
+            "Initial position: [ID:%03d] PresPos:%d PresSpd:%d"
+            % (STS_ID, sts_present_position, sts_present_speed)
+        )
+
+        TrenutnaPos = sts_present_position
+        ProslaPos = sts_present_position
+        PredjeniPut = TrenutnaPos - ProslaPos
+        opterecenje = 0
+        packetHandler.SetMaxLoad(STS_ID, 80)
+
+        while True:
+            # Write STServo goal position/moving speed/moving acc
+            sts_comm_result, sts_error = packetHandler.WriteSpec(
+                STS_ID, STS_MOVING_SPEED, self.STS_MOVING_ACC
+            )
+            if sts_comm_result != COMM_SUCCESS:
+                print("%s" % packetHandler.getTxRxResult(sts_comm_result))
+            if sts_error != 0:
+                print("%s" % packetHandler.getRxPacketError(sts_error))
+
+            ProslaPos = sts_present_position
+            sts_present_position, sts_present_speed, sts_comm_result, sts_error = (
+                packetHandler.ReadPosSpeed(STS_ID)
+            )
+            sts_present_load, sts_comm_result, sts_error = packetHandler.ReadLoad(
+                STS_ID
+            )
+            sts_max_load, sts_comm_result, sts_error = packetHandler.MaxLoad(STS_ID)
+            opterecenje = (sts_present_load & ((1 << 10) - 1)) * 0.1
+            max_opterecenje = sts_max_load & 255
+            print(
+                "Current position: [ID:%03d] PresPos:%d PresSpd:%d Load:%d Max:%d"
+                % (
+                    STS_ID,
+                    sts_present_position,
+                    sts_present_speed,
+                    opterecenje,
+                    max_opterecenje,
+                )
+            )
+
+            if opterecenje > 75:
+                print(
+                    "High load detected: [ID:%03d] PresPos:%d PresSpd:%d Load:%d"
+                    % (STS_ID, sts_present_position, sts_present_speed, opterecenje)
+                )
+                sts_comm_result, sts_error = packetHandler.WriteSpec(STS_ID, 0, 0)
+                if sts_comm_result != COMM_SUCCESS:
+                    print("%s" % packetHandler.getTxRxResult(sts_comm_result))
+                if sts_error != 0:
+                    print("%s" % packetHandler.getRxPacketError(sts_error))
+                break
+            TrenutnaPos = sts_present_position
+            if abs(TrenutnaPos - ProslaPos) > 3000:
+                PredjeniPut = PredjeniPut
+            else:
+                PredjeniPut = PredjeniPut + abs(TrenutnaPos - ProslaPos)
+
+            print("Distance traveled: %d deg " % (PredjeniPut * 360 / 4096))
+            if PredjeniPut >= TargetPos:
+                print(
+                    "Target position reached: [ID:%03d] PresPos:%d PresSpd:%d"
+                    % (STS_ID, sts_present_position, sts_present_speed)
+                )
+                sts_comm_result, sts_error = packetHandler.WriteSpec(STS_ID, 0, 0)
+                if sts_comm_result != COMM_SUCCESS:
+                    print("%s" % packetHandler.getTxRxResult(sts_comm_result))
+                if sts_error != 0:
+                    print("%s" % packetHandler.getRxPacketError(sts_error))
+                break
+
+        # Close port
+        portHandler.closePort()

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/package.xml

@@ -19,7 +19,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>toid_msgs</depend>
   <depend>visualization_msgs</depend>
-  <depend>rplidar</depend>
+  <depend>rplidar_ros</depend>
 
 
 

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

@@ -15,9 +15,11 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/ActiveElements.msg"
   "msg/Rival.msg"
   "srv/SendDouble.srv"
+  "srv/SendString.srv"
   "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_msgs/srv/SendString.srv

@@ -0,0 +1,2 @@
+string text
+---

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;
+}