finished toid_interaction node

.gitignore

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

.gitmodules

@@ -1,6 +1,3 @@
 [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,33 +1,15 @@
-FROM ros:jazzy-perception
+FROM ros:jazzy-ros-base
 
 ENV DEBIAN_FRONTEND=noninteractive
 
 # ---------- System dependencies ----------
-RUN apt-get update && apt-get upgrade && apt-get install -y \
+RUN apt-get update && 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
@@ -40,20 +22,13 @@ 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 --skip-keys=libcamera \
+    rosdep install --from-paths ./ --ignore-src -r -y \
     && rm -rf /var/lib/apt/lists/*
 
 RUN rm -rf ./*

README.md

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

docker-compose.yaml

@@ -8,30 +8,11 @@ services:
     network_mode: host
 
     volumes:
-      - ./:/ros_ws
+      - ./:/ros_ws/src
       - /dev/:/dev/
       - /sys/:/sys/
-      - /run/udev/:/run/udev/
 
     entrypoint: ["sleep","infinity"]
 
     profiles:
-      - base
+      - 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

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

toid_behaviors/CMakeLists.txt

@@ -32,8 +32,6 @@ 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

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

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

@@ -1,80 +0,0 @@
-#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,20 +127,17 @@ public:
 
   bool check_rival_collision(geometry_msgs::msg::Pose2D & pose)
   {
-    if (!rivals_ || (this->clock_->now() - rivals_->header.stamp ) > rclcpp::Duration::from_seconds(1.0)) {
+    if (!rivals_) {
       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.x = dx * cosp + dy * sinp;
-      local_rival.y = dx * sinp + dy * cosp;
+      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;
@@ -151,7 +148,6 @@ 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;
       }
@@ -170,7 +166,7 @@ protected:
 
   double robot_width_ = 0.30;
   double robot_length_ = 0.30;
-  double rival_radius_ = 0.22;
+  double rival_radius_ = 0.30;
 
   Rival::SharedPtr rivals_;
   rclcpp::Subscription<Rival>::SharedPtr rivals_sub_;

toid_behaviors/src/approach_acorns.cpp

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

@@ -1,234 +0,0 @@
-#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,11 +9,5 @@
         <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,14 +1,13 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 138
+    Help Height: 78
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
         - /Status1
-        - /Pose1
       Splitter Ratio: 0.5
-    Tree Height: 732
+    Tree Height: 542
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -75,14 +74,6 @@ 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
@@ -127,10 +118,6 @@ Visualization Manager:
           Value: true
         base_link:
           Value: true
-        camera:
-          Value: true
-        camera_base:
-          Value: true
         drivewhl_l_link:
           Value: true
         drivewhl_r_link:
@@ -147,43 +134,21 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        base_footprint:
+        base_link:
-          base_link:
+          base_footprint:
-            drivewhl_l_link:
+            {}
-              {}
+          drivewhl_l_link:
-            drivewhl_r_link:
+            {}
-              {}
+          drivewhl_r_link:
-            left_caster:
+            {}
-              {}
+          left_caster:
-            lidar:
+            {}
-              {}
+          lidar:
-            right_caster:
+            {}
-              {}
+          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
@@ -230,7 +195,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/XYOrbit
-      Distance: 1.2598285675048828
+      Distance: 1.59040105342865
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -245,18 +210,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.015398791059851646
+      Pitch: 0.6053992509841919
       Target Frame: <Fixed Frame>
       Value: XYOrbit (rviz_default_plugins)
-      Yaw: 0.09356015175580978
+      Yaw: 1.088563323020935
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1250
+  Height: 846
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000027a000003d8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000003d80000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b000003d8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000003d80000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007620000005efc0100000002fb0000000800540069006d00650100000000000007620000048700fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000003d800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016c000002acfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002ac000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002ac000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002de00fffffffb0000000800540069006d0065010000000000000450000000000000000000000229000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -265,6 +230,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1890
+  Width: 1200
   X: 60
-  Y: 64
+  Y: 60

toid_bot_description/src/toid_bot_control.xacro

@@ -1,13 +1,12 @@
 <?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 serial_port:=/dev/ttyACM0">
+  <xacro:macro name="toid_control" params="name:=toid_control use_mock_hardware:=^|true">
 
     <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,7 +2,6 @@
 <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"/>
@@ -12,7 +11,7 @@
 <xacro:property name="wheel_width" value="0.03"/>
 
 <xacro:property name="wheel_zoff" value="0.03"/>
-<xacro:property name="wheel_xoff" value="-0.105"/>
+<xacro:property name="wheel_xoff" value="-0.07"/>
 <xacro:property name="wheel_inset" value="0.01"/>
 
 <xacro:property name="caster_radius" value="0.02"/>
@@ -23,11 +22,7 @@
 
 <xacro:property name="lidar_radius" value="0.03"/>
 <xacro:property name="lidar_height" value="0.02"/>
-<xacro:property name="lidar_xoff" value="0.00"/>
+<xacro:property name="lidar_xoff" value="0.09"/>
-
-<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}"/>
 
@@ -44,9 +39,6 @@
 
 <link name="base_footprint"/>
 
-<link name="camera" />
-
-
 <link name="lidar">
     <visual>
         <geometry>
@@ -64,16 +56,10 @@
     <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 ${pi}"/>
+    <origin xyz="${lidar_xoff} 0 ${(base_height + lidar_height)/2}" rpy="0 0 0"/>
 </joint>
 
 
@@ -126,6 +112,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)" serial_port="$(arg serial_port)"/>
+<xacro:toid_control name="toid_bot_control" use_mock_hardware="$(arg use_mock)"/>
 
 </robot>

toid_bt/behavior_trees/behavior1.xml

@@ -1,16 +1,9 @@
 <?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"
+      <Seq2 text="0101"
             service_name=""/>
       <Seq3 service_name=""/>
     </Sequence>
@@ -19,240 +12,21 @@
   <BehaviorTree ID="seq1">
     <Sequence>
       <ZeroOdom service_name=""/>
-      <SetInitialPose y="0.805"
+      <MovePointSimple x="1.05"
-                      theta="-90"
+                       y="0"
-                      frame_id="map"
+                       theta="0"
-                      x="1.325"
-                      topic_name="__default__placeholder__"/>
-      <WaitPullPin action_name=""/>
-      <Timeout msec="100000">
-        <Sequence>
-          <InPose timeout="1.000000">
-            <MovePointSimple x="1.325"
-                             y="0.200"
-                             theta="-90"
-                             max_speed="0.200000"
-                             backwards="false"
-                             action_name=""/>
-          </InPose>
-          <Seq1 service_name=""/>
-          <Seq2 text=""
-                service_name=""/>
-          <MovePointSimple x="1.325"
-                           y="-0.05"
-                           theta="-90"
-                           max_speed="0.200000"
-                           backwards="false"
-                           action_name=""/>
-          <Seq3 service_name=""/>
-          <MovePointSimple x="1.325"
-                           y="0.805"
-                           theta="-90"
-                           max_speed="0.200000"
-                           backwards="true"
-                           action_name=""/>
-        </Sequence>
-      </Timeout>
-    </Sequence>
-  </BehaviorTree>
-
-  <BehaviorTree ID="seq2">
-    <Sequence>
-      <ZeroOdom service_name=""/>
-      <SetInitialPose y="0.955"
-                      theta="-90"
-                      frame_id="map"
-                      x="1.330"
-                      topic_name="__default__placeholder__"/>
-      <MovePointSimple x="1.330"
-                       y="0.805"
-                       theta="-90"
                        max_speed="0.300000"
                        backwards="false"
                        action_name=""/>
-      <WaitPullPin action_name=""/>
+      <MovePointSimple x="0.76"
-      <Timeout msec="100000">
+                       y="0.18"
-        <Sequence>
+                       theta="-90"
-          <InPose timeout="1.000000">
+                       max_speed="0.250000"
-            <MovePointSimple x="1.33"
+                       backwards="true"
-                             y="0.354"
+                       action_name=""/>
-                             theta="-90"
+      <TranslateX x="0.5"
-                             max_speed="0.300000"
+                  max_speed="0.300000"
-                             backwards="false"
+                  action_name=""/>
-                             action_name=""/>
-          </InPose>
-          <Seq1 service_name=""/>
-          <Parallel failure_count="1"
-                    success_count="-1">
-            <Seq2 text=""
-                  service_name=""/>
-            <MovePointSimple x="0.795"
-                             y="-0.2"
-                             theta="180"
-                             max_speed="0.500000"
-                             backwards="false"
-                             action_name=""/>
-          </Parallel>
-          <Parallel failure_count="1"
-                    success_count="-1">
-            <Seq3 service_name=""/>
-            <Sequence>
-              <Sleep msec="500"/>
-              <TranslateX x="-0.3"
-                          max_speed="0.000000"
-                          action_name=""/>
-              <RotateTowards x="1.33"
-                             y="0.354"
-                             backwards="true"
-                             max_speed="2.000000"
-                             min_angle="0.000000"
-                             action_name=""/>
-            </Sequence>
-          </Parallel>
-          <MovePointSimple x="1.330"
-                           y="0.354"
-                           theta="-90"
-                           max_speed="0.400000"
-                           backwards="true"
-                           action_name=""/>
-          <MovePointSimple x="1.330"
-                           y="-0.52"
-                           theta="-90"
-                           max_speed="0.000000"
-                           backwards="false"
-                           action_name=""/>
-          <Seq1 service_name=""/>
-          <ParallelAll max_failures="1">
-            <Seq2 text=""
-                  service_name=""/>
-            <MovePointSimple x="1.33"
-                             y="-0.2"
-                             theta="-90"
-                             max_speed="0.200000"
-                             backwards="false"
-                             action_name=""/>
-          </ParallelAll>
-          <Parallel failure_count="1"
-                    success_count="-1">
-            <Seq3 service_name=""/>
-            <Sequence>
-              <Sleep msec="500"/>
-              <MovePointSimple x="1.33"
-                               y="0.354"
-                               theta="-90"
-                               max_speed="0.400000"
-                               backwards="true"
-                               action_name=""/>
-              <MovePointSimple x="1.37"
-                               y="0.48"
-                               theta="-70"
-                               max_speed="0.400000"
-                               backwards="true"
-                               action_name=""/>
-            </Sequence>
-          </Parallel>
-          <RotateTowards x="1.003"
-                         y="-0.029"
-                         backwards="false"
-                         max_speed="1.300000"
-                         min_angle="0.000000"
-                         action_name=""/>
-          <MovePointSimple x="1.003"
-                           y="-0.029"
-                           theta="-90"
-                           max_speed="0.400000"
-                           backwards="false"
-                           action_name=""/>
-        </Sequence>
-      </Timeout>
-    </Sequence>
-  </BehaviorTree>
-
-  <BehaviorTree ID="seq2_yellow">
-    <Sequence>
-      <ZeroOdom service_name=""/>
-      <SetInitialPose y="0.806"
-                      theta="-90"
-                      frame_id="map"
-                      x="-1.328"
-                      topic_name="__default__placeholder__"/>
-      <WaitPullPin action_name=""/>
-      <Timeout msec="100000">
-        <Sequence>
-          <InPose timeout="1.000000">
-            <MovePointSimple x="-1.328"
-                             y="0.354"
-                             theta="-90"
-                             max_speed="0.300000"
-                             backwards="false"
-                             action_name=""/>
-          </InPose>
-          <Seq1 service_name=""/>
-          <Parallel failure_count="1"
-                    success_count="-1">
-            <Seq2 text=""
-                  service_name=""/>
-            <MovePointSimple x="-0.795"
-                             y="-0.2"
-                             theta="0"
-                             max_speed="0.500000"
-                             backwards="false"
-                             action_name=""/>
-          </Parallel>
-          <Parallel failure_count="1"
-                    success_count="-1">
-            <Seq3 service_name=""/>
-            <Sequence>
-              <Sleep msec="500"/>
-              <TranslateX x="-0.3"
-                          max_speed="0.000000"
-                          action_name=""/>
-              <RotateTowards x="-1.328"
-                             y="0.354"
-                             backwards="true"
-                             max_speed="2.000000"
-                             min_angle="0.000000"
-                             action_name=""/>
-            </Sequence>
-          </Parallel>
-          <MovePointSimple x="-1.328"
-                           y="0.354"
-                           theta="-90"
-                           max_speed="0.400000"
-                           backwards="true"
-                           action_name=""/>
-          <Seq1 service_name=""/>
-          <ParallelAll max_failures="1">
-            <Seq2 text=""
-                  service_name=""/>
-            <MovePointSimple x="-1.328"
-                             y="-0.2"
-                             theta="-90"
-                             max_speed="0.200000"
-                             backwards="false"
-                             action_name=""/>
-          </ParallelAll>
-          <Parallel failure_count="1"
-                    success_count="-1">
-            <Seq3 service_name=""/>
-            <Sequence>
-              <Sleep msec="500"/>
-              <MovePointSimple x="-1.328"
-                               y="0.354"
-                               theta="-90"
-                               max_speed="0.400000"
-                               backwards="true"
-                               action_name=""/>
-            </Sequence>
-          </Parallel>
-          <MovePointSimple x="-1.328"
-                           y="0.806"
-                           theta="-90"
-                           max_speed="0.400000"
-                           backwards="true"
-                           action_name=""/>
-        </Sequence>
-      </Timeout>
     </Sequence>
   </BehaviorTree>
 
@@ -261,7 +35,6 @@
       <DetectStuck timeout="1.000000">
         <RotateTowards x="0.4"
                        y="0.0"
-                       backwards="false"
                        max_speed="0.000000"
                        min_angle="0.000000"
                        action_name=""/>
@@ -276,11 +49,6 @@
                   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"/>
@@ -302,9 +70,6 @@
                   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"/>
@@ -328,23 +93,6 @@
       <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"/>
@@ -354,10 +102,6 @@
       <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,7 +4,6 @@
     <Sequence>
       <SetBlackboard value="0.265"
                      output_key="width"/>
-      <WaitPullPin />
       <Sleep msec="1000"/>
       <ZeroOdom service_name=""/>
       <RotateTowards x="0.4"
@@ -23,7 +22,7 @@
         <Sleep msec="500"/>
         <RotateSimple angle="0"
                       max_speed="0.500000"
-                      min_angle="700"
+                      min_angle="270"
                       action_name=""/>
       </Sequence>
       <ForceSuccess>
@@ -37,7 +36,7 @@
         </DetectStuck>
       </ForceSuccess>
       <SetWidth width="{width}"
-                count="2"
+                count="1"
                 new_width="{width}"
                 service_name=""/>
     </Sequence>
@@ -47,57 +46,31 @@
     <Sequence>
       <Sleep msec="5000"/>
       <ZeroOdom service_name=""/>
-      <WaitPullPin />
       <Sleep msec="1000"/>
-      <Repeat num_cycles="5">
+      <Sequence>
-        <Sequence>
+        <MovePointSimple x="1.1"
-          <MovePointSimple x="1.1"
+                         y="0"
-                           y="0"
+                         theta="0"
-                           theta="0"
+                         max_speed="0.10000"
-                           max_speed="0.30000"
+                         backwards="false"
-                           backwards="false"
+                         action_name=""/>
-                           action_name=""/>
+        <Sleep msec="1000"/>
-          <Sleep msec="1000"/>
+        <RotateSimple angle="180"
-          <RotateSimple angle="180"
+                      max_speed="0.300000"
-                        max_speed="1.300000"
+                      min_angle="10"
-                        min_angle="10"
+                      action_name=""/>
-                        action_name=""/>
+        <Sleep msec="500"/>
-          <Sleep msec="500"/>
+        <MovePointSimple x="0.35"
-          <MovePointSimple x="0.35"
+                         y="0"
-                           y="0"
+                         theta="180"
-                           theta="180"
+                         max_speed="0.100000"
-                           max_speed="0.300000"
+                         backwards="false"
-                           backwards="false"
+                         action_name=""/>
-                           action_name=""/>
+        <RotateSimple angle="0"
-          <RotateSimple angle="0"
+                      max_speed="0.300000"
-                        max_speed="1.300000"
+                      min_angle="-10"
-                        min_angle="-10"
+                      action_name=""/>
-                        action_name=""/>
+      </Sequence>
-          <Sleep msec="500"/>
-          <MovePointSimple x="1.1"
-                           y="0"
-                           theta="0"
-                           max_speed="0.30000"
-                           backwards="false"
-                           action_name=""/>
-          <Sleep msec="500"/>
-          <RotateSimple angle="180"
-                        max_speed="1.300000"
-                        min_angle="-10"
-                        action_name=""/>
-          <Sleep msec="500"/>
-          <MovePointSimple x="0.35"
-                           y="0"
-                           theta="180"
-                           max_speed="0.300000"
-                           backwards="false"
-                           action_name=""/>
-          <RotateSimple angle="0"
-                        max_speed="1.300000"
-                        min_angle="10"
-                        action_name=""/>
-        </Sequence>
-      </Repeat>
       <ForceSuccess>
         <DetectStuck timeout="1.000000">
           <MovePointSimple x="-0.2"
@@ -115,7 +88,6 @@
 
   <BehaviorTree ID="wheel_size">
     <Sequence>
-      <WaitPullPin />
       <Sleep msec="1000"/>
       <ZeroOdom service_name=""/>
       <Sleep msec="1000"/>
@@ -198,4 +170,4 @@
     </Action>
   </TreeNodesModel>
 
-</root>
+</root>

toid_bt/behavior_trees/toid_behaviors.btproj

@@ -4,23 +4,12 @@
     <include path="calibration_routines.xml"/>
     <!-- Description of Node Models (used by Groot) -->
     <TreeNodesModel>
-        <Action ID="ApproachAcorns">
-            <input_port name="x" type="double"/>
-            <input_port name="y" type="double"/>
-            <input_port name="theta" type="double"/>
-            <input_port name="max_speed" default="0.000000" type="double"/>
-            <input_port name="backwards" default="false" type="bool"/>
-            <input_port name="action_name" type="std::string">Action server name</input_port>
-        </Action>
         <Decorator ID="DetectStuck">
             <input_port name="timeout" default="1.000000" type="double"/>
         </Decorator>
         <Action ID="EndCalib">
             <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
-        <Decorator ID="InPose">
-            <input_port name="timeout" default="1.000000" type="double"/>
-        </Decorator>
         <Action ID="MovePointSimple">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
@@ -29,12 +18,6 @@
             <input_port name="backwards" default="false" type="bool"/>
             <input_port name="action_name" type="std::string">Action server name</input_port>
         </Action>
-        <Action ID="RotateAcorns">
-            <input_port name="angle" type="double"/>
-            <input_port name="max_speed" default="0.000000" type="double"/>
-            <input_port name="min_angle" default="0.000000" type="double"/>
-            <input_port name="action_name" type="std::string">Action server name</input_port>
-        </Action>
         <Action ID="RotateSimple">
             <input_port name="angle" type="double"/>
             <input_port name="max_speed" default="0.000000" type="double"/>
@@ -44,7 +27,6 @@
         <Action ID="RotateTowards">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
-            <input_port name="backwards" default="false" type="bool"/>
             <input_port name="max_speed" default="0.000000" type="double"/>
             <input_port name="min_angle" default="0.000000" type="double"/>
             <input_port name="action_name" type="std::string">Action server name</input_port>
@@ -59,18 +41,6 @@
         <Action ID="Seq3">
             <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
-        <Condition ID="SetInitialPose">
-            <input_port name="y" default="0.000000" type="double">Y position in meters</input_port>
-            <input_port name="theta" default="0.000000" type="double">Heading in degrees</input_port>
-            <input_port name="frame_id" default="map" type="std::string">Reference frame</input_port>
-            <input_port name="x" default="0.000000" type="double">X position in meters</input_port>
-            <input_port name="topic_name" default="__default__placeholder__" type="std::string">Topic name</input_port>
-        </Condition>
-        <Action ID="SetParameter">
-            <input_port name="parameter" type="std::string"/>
-            <input_port name="value" type="std::string"/>
-            <input_port name="node" type="std::string"/>
-        </Action>
         <Action ID="SetWidth">
             <input_port name="width" type="double"/>
             <input_port name="count" default="1" type="int"/>
@@ -82,9 +52,6 @@
             <input_port name="max_speed" default="0.000000" type="double"/>
             <input_port name="action_name" type="std::string">Action server name</input_port>
         </Action>
-        <Action ID="WaitPullPin">
-            <input_port name="action_name" type="std::string">Action server name</input_port>
-        </Action>
         <Action ID="ZeroOdom">
             <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>

toid_bt/include/toid_bt/bt_executor.hpp

@@ -8,7 +8,6 @@
 #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 {
@@ -22,12 +21,6 @@ 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:
@@ -35,12 +28,6 @@ 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,5 +5,4 @@
 
 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

@@ -1,74 +0,0 @@
-#pragma once
-
-#include "angles/angles.h"
-#include "behaviortree_ros2/bt_service_node.hpp"
-#include "std_srvs/srv/empty.hpp"
-#include "tf2/utils.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-#include "toid_bt/plugin.hpp"
-
-namespace toid
-{
-
-class 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

@@ -1,48 +0,0 @@
-#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,7 +25,6 @@ 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"),
@@ -38,17 +37,11 @@ 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);
-      goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x);
-    }
-    if(backwards) {
-      goal.angle = std::atan2(pose.pose.position.y - y, pose.pose.position.x - x);
-    }
     goal.min_angle = angles::from_degrees(min_angle);
     goal.max_speed = max_speed;
     goal.mode = 1;

toid_bt/include/toid_bt/plugins/send_text_action.hpp

@@ -15,8 +15,8 @@ class SendTextNode : public BT::RosServiceNode<toid_msgs::srv::SendString>
 {
 public:
   SendTextNode(
-    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, FlipFunc get_text)
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
-  : BT::RosServiceNode<toid_msgs::srv::SendString>(name, conf, params), get_text(get_text)
+  : BT::RosServiceNode<toid_msgs::srv::SendString>(name, conf, params)
   {
   }
 
@@ -29,9 +29,6 @@ public:
 
   bool setRequest(typename Request::SharedPtr &req) override { 
     std::string text = getInput<std::string>("text").value_or("");
-    if(text=="") {
-      get_text(text);
-    }
     req->text = text;
 
     return true;
@@ -41,8 +38,6 @@ public:
   {
     return BT::NodeStatus::SUCCESS;
   }
-protected:
-  FlipFunc get_text;
 };
 
 }  // namespace toid

toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp

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

@@ -1,50 +0,0 @@
-#pragma once
-
-#include "angles/angles.h"
-#include "behaviortree_ros2/bt_service_node.hpp"
-#include "std_srvs/srv/empty.hpp"
-#include "tf2/utils.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
-#include "toid_bt/plugin.hpp"
-#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_move_coords.hpp"
+#include "toid_msgs/action/simple_translate_x.hpp"
 
 namespace toid
 {
 
-class TranslateXNode : public BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>
+class TranslateXNode : public BT::RosActionNode<toid_msgs::action::SimpleTranslateX>
 {
 public:
   TranslateXNode(
     const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
     PoseFunc pose)
-  : BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>(name, conf, params), get_pose(pose)
+  : BT::RosActionNode<toid_msgs::action::SimpleTranslateX>(name, conf, params), get_pose(pose)
   {
   }
 
@@ -34,16 +34,7 @@ public:
     auto x_goal = getInput<double>("x").value();
     auto max_speed = getInput<double>("max_speed").value();
 
-    geometry_msgs::msg::PoseStamped pose;
+    goal.distance = x_goal;
-    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,15 +6,11 @@
 #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
@@ -36,13 +32,6 @@ 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)
@@ -52,62 +41,37 @@ 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, "/moveCoords"), get_pose);
+    "TranslateX", BT::RosNodeParams(nh, "/translateX"), 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");
+  factory.registerNodeType<toid::EmptySrvNode>("Seq1", BT::RosNodeParams(nh, "/sequence1"));
-  service_params1.server_timeout = std::chrono::seconds(15);
-  factory.registerNodeType<toid::EmptySrvNode>("Seq1", service_params1);
 
-  BT::RosNodeParams service_params2(nh, "/sequence2");
+  BT::RosNodeParams service_params(nh, "/sequence2");
-  service_params2.server_timeout = std::chrono::seconds(20);
+  service_params.server_timeout = std::chrono::seconds(15);
-  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params2, get_to_flip);
+  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params);
 
-  BT::RosNodeParams service_params3(nh, "/sequence3");
+  factory.registerNodeType<toid::EmptySrvNode>("Seq3", BT::RosNodeParams(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;
 }
 
@@ -116,23 +80,6 @@ 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

@@ -1,106 +1,97 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition, UnlessCondition
+from launch.conditions import IfCondition, UnlessCondition 
 from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution
 from launch_ros.actions import Node, LifecycleNode
 from launch_ros.substitutions import FindPackageShare
 import os
 
-
 def generate_launch_description():
 
-    pkg_share = FindPackageShare("").find("toid_control")
+    pkg_share = FindPackageShare("").find('toid_control')
-    params = os.path.join(pkg_share, "params", "toid_general_params.yaml")
+    params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
-    default_rviz_config_path = os.path.join(pkg_share, "rviz", "rviz.rviz")
+    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'rviz.rviz')
 
-    description_pkg_share = FindPackageShare("").find("toid_bot_description")
+    description_pkg_share = FindPackageShare("").find('toid_bot_description')
     default_model_path = os.path.join(
-        description_pkg_share, "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",
+        package='toid_odometry',
-        executable="toid_odometry",
+        executable='toid_odometry',
-        name="map_to_odom_broadcaster",
+        name='map_to_odom_broadcaster',
         emulate_tty=True,
-        parameters=[
+        parameters=[{'mock_odom': use_mock}],
-            {
-                "mock_odom": use_mock,
-                "serial_port": "/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if02",
-            }
-        ],
     )
 
+
     robot_state_publisher = Node(
-        package="robot_state_publisher",
+        package='robot_state_publisher',
-        executable="robot_state_publisher",
+        executable='robot_state_publisher',
-        name="robot_state_publisher",
+        name='robot_state_publisher',
-        output="screen",
+        output='screen',
         emulate_tty=True,
-        parameters=[
+        parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}]
-            {
-                "robot_description": Command(
-                    [
-                        "xacro ",
-                        default_model_path,
-                        " use_mock:=",
-                        use_mock,
-                        " serial_port:=/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if00",
-                    ]
-                )
-            }
-        ],
     )
 
     controller_manager = Node(
-        package="controller_manager",
+        package='controller_manager',
-        executable="ros2_control_node",
+        executable='ros2_control_node',
-        output="screen",
+        output='screen',
         emulate_tty=True,
         parameters=[params],
-        arguments=["--ros-args", "--log-level", "warn"],
+        arguments=['--ros-args', '--log-level', 'warn']
     )
 
     joint_state_broadcaster = Node(
-        package="controller_manager",
+        package='controller_manager',
-        executable="spawner",
+        executable='spawner',
-        output="screen",
+        output='screen',
         emulate_tty=True,
-        arguments=["joint_state_broadcaster", "--ros-args", "--log-level", "warn"],
+        arguments=[
+            "joint_state_broadcaster",
+            '--ros-args', '--log-level', 'warn'
+            ]
     )
 
     velocity_controller = Node(
-        package="controller_manager",
+        package='controller_manager',
-        executable="spawner",
+        executable='spawner',
-        output="screen",
+        output='screen',
         emulate_tty=True,
         arguments=[
             "velocity_controller",
             "--inactive",
             "-p",
             params,
-            "--ros-args",
+            '--ros-args', '--log-level', 'warn'
-            "--log-level",
+            ],
-            "warn",
-        ],
     )
 
+
     diffbot_base_controller = Node(
-        package="controller_manager",
+        package='controller_manager',
-        executable="spawner",
+        executable='spawner',
-        output="both",
+        output='both',
         emulate_tty=True,
         arguments=[
             "diffdrive_controller",
@@ -111,43 +102,39 @@ def generate_launch_description():
             "--controller-ros-args",
             "-r diffdrive_controller/odom:=/odom",
             "--controller-ros-args",
-            IfElseSubstitution(
+            IfElseSubstitution(use_mock, 
-                use_mock,
+                "--param odom_frame_id:=odom", 
-                "--param odom_frame_id:=odom",
+                "--param odom_frame_id:=odom_expected"
-                "--param odom_frame_id:=odom_expected",
             ),
             "--controller-ros-args",
-            IfElseSubstitution(
+            IfElseSubstitution(use_mock, 
-                use_mock,
+                "--param enable_odom_tf:=true", 
-                "--param enable_odom_tf:=true",
+                "--param enable_odom_tf:=false"
-                "--param enable_odom_tf:=false",
             ),
-            "--ros-args",
+            '--ros-args', '--log-level', 'warn'
-            "--log-level",
+        ]
-            "warn",
-        ],
     )
 
     rviz_node = Node(
-        package="rviz2",
+        package='rviz2',
-        executable="rviz2",
+        executable='rviz2',
-        name="rviz2",
+        name='rviz2',
-        output="screen",
+        output='screen',
         emulate_tty=True,
-        arguments=["-d", default_rviz_config_path, "--ros-args", "--log-level", "warn"],
+        arguments=['-d', default_rviz_config_path, 
-        condition=IfCondition(visualize),
+                   '--ros-args', '--log-level', 'warn'
+                   ],
+        condition=IfCondition(visualize)
     )
 
-    return LaunchDescription(
+    return LaunchDescription([
-        [
+        visualize_arg,
-            visualize_arg,
+        use_mock_arg,
-            use_mock_arg,
+        odom_broadcast,
-            odom_broadcast,
+        robot_state_publisher,
-            robot_state_publisher,
+        controller_manager,
-            controller_manager,
+        joint_state_broadcaster,
-            joint_state_broadcaster,
+        diffbot_base_controller,
-            diffbot_base_controller,
+        velocity_controller,
-            velocity_controller,
+        rviz_node
-            rviz_node,
+    ])
-        ]
-    )

toid_control/src/toid_control.cpp

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

toid_interaction/package.xml

@@ -13,8 +13,6 @@
   <test_depend>python3-pytest</test_depend>
 
   <depend>python3-serial</depend>
-  <depend>python3-gpiozero</depend>
-  <depend>toid_msgs</depend>
 
   <export>
     <build_type>ament_python</build_type>

toid_interaction/setup.py

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

toid_interaction/toid_interaction/interaction_node.py

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

toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py

@@ -75,10 +75,6 @@ class sts(protocol_packet_handler):
     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

toid_interaction/toid_interaction/mechanism/motori.py

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

toid_interaction/toid_interaction/mechanism/sekvenca_2026.py

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

toid_interaction/toid_interaction/mechanism/zidovi.py

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

toid_lidar/launch/launch.py

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

toid_lidar/src/toid_lidar_node.cpp

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

toid_msgs/CMakeLists.txt

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

toid_msgs/action/EmptyAction.action

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

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, AllSubstitution
+from launch.substitutions import  LaunchConfiguration
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -10,8 +10,6 @@ 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')
@@ -39,20 +37,6 @@ def generate_launch_description():
         default_value='True',
         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(
@@ -65,35 +49,6 @@ 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',
@@ -163,11 +118,6 @@ 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,28 +22,19 @@ 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", "approachAcorns", "rotateAcorns"]
+    behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords"]
     spin:
       plugin: "nav2_behaviors::Spin"
     backup:
       plugin: "nav2_behaviors::BackUp"
     rotate:
       plugin: "toid::SimpleRotateBehavior"
-      max_angular_accel: 3.0
+      max_angular_accel: 4.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
@@ -61,7 +52,7 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_footprint
-      footprint: "[[-0.045, 0.15], [0.255, 0.15], [0.255, -0.15], [-0.045, -0.15]]"
+      footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]"
       footprint_padding: 0.02
       track_unknown_space: false
       rolling_window: false
@@ -82,7 +73,7 @@ local_costmap:
     ros__parameters:
       update_frequency: 5.0
       publish_frequency: 2.0
-      footprint: "[[-0.045, 0.15], [0.255, 0.15], [0.255, -0.15], [-0.045, -0.15]]"
+      footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]"
       footprint_padding: 0.01
       #robot_radius: 0.18
       global_frame: map
@@ -92,14 +83,11 @@ local_costmap:
       height: 1
       resolution: 0.01
       introspection_mode: "disabled"
-      plugins: ["static_layer", "rival_layer", "inflation_layer"]
+      plugins: ["static_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,14 +1,13 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 138
+    Help Height: 78
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /RobotModel1
         - /GlobalCostMap1/Topic1
-        - /Pose1
       Splitter Ratio: 0.5
-    Tree Height: 668
+    Tree Height: 592
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -75,10 +74,6 @@ 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
@@ -252,39 +247,6 @@ 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
@@ -331,33 +293,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/XYOrbit
-      Distance: 2.2019217014312744
+      Distance: 3.863811731338501
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.3222081661224365
+        X: 0.1778966784477234
-        Y: -0.08033189922571182
+        Y: -1.1747734546661377
-        Z: 0
+        Z: 1.7285346984863281e-06
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.064796805381775
+      Pitch: 1.3697962760925293
       Target Frame: <Fixed Frame>
       Value: XYOrbit (rviz_default_plugins)
-      Yaw: 4.925206184387207
+      Yaw: 4.628584861755371
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1186
+  Height: 896
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000025400000398fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000003980000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b00000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000003980000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070c0000005efc0100000002fb0000000800540069006d006501000000000000070c0000048700fffffffb0000000800540069006d00650100000000000004500000000000000000000003450000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000254000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002de000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002de000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005d30000003efc0100000002fb0000000800540069006d00650100000000000005d30000027b00fffffffb0000000800540069006d0065010000000000000450000000000000000000000264000002de00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -366,6 +328,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1804
+  Width: 1491
-  X: 428
+  X: 429
-  Y: 74
+  Y: 75

toid_odometry/src/odometry.cpp

@@ -139,23 +139,6 @@ 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};
@@ -163,7 +146,6 @@ 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) {
@@ -173,7 +155,6 @@ 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>) {
@@ -185,15 +166,12 @@ 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() {
@@ -225,7 +203,6 @@ 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;
@@ -267,17 +244,6 @@ 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

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

@@ -1,202 +0,0 @@
-
-                                 Apache License
-                           Version 2.0, January 2004
-                        http://www.apache.org/licenses/
-
-   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
-
-   1. Definitions.
-
-      "License" shall mean the terms and conditions for use, reproduction,
-      and distribution as defined by Sections 1 through 9 of this document.
-
-      "Licensor" shall mean the copyright owner or entity authorized by
-      the copyright owner that is granting the License.
-
-      "Legal Entity" shall mean the union of the acting entity and all
-      other entities that control, are controlled by, or are under common
-      control with that entity. For the purposes of this definition,
-      "control" means (i) the power, direct or indirect, to cause the
-      direction or management of such entity, whether by contract or
-      otherwise, or (ii) ownership of fifty percent (50%) or more of the
-      outstanding shares, or (iii) beneficial ownership of such entity.
-
-      "You" (or "Your") shall mean an individual or Legal Entity
-      exercising permissions granted by this License.
-
-      "Source" form shall mean the preferred form for making modifications,
-      including but not limited to software source code, documentation
-      source, and configuration files.
-
-      "Object" form shall mean any form resulting from mechanical
-      transformation or translation of a Source form, including but
-      not limited to compiled object code, generated documentation,
-      and conversions to other media types.
-
-      "Work" shall mean the work of authorship, whether in Source or
-      Object form, made available under the License, as indicated by a
-      copyright notice that is included in or attached to the work
-      (an example is provided in the Appendix below).
-
-      "Derivative Works" shall mean any work, whether in Source or Object
-      form, that is based on (or derived from) the Work and for which the
-      editorial revisions, annotations, elaborations, or other modifications
-      represent, as a whole, an original work of authorship. For the purposes
-      of this License, Derivative Works shall not include works that remain
-      separable from, or merely link (or bind by name) to the interfaces of,
-      the Work and Derivative Works thereof.
-
-      "Contribution" shall mean any work of authorship, including
-      the original version of the Work and any modifications or additions
-      to that Work or Derivative Works thereof, that is intentionally
-      submitted to Licensor for inclusion in the Work by the copyright owner
-      or by an individual or Legal Entity authorized to submit on behalf of
-      the copyright owner. For the purposes of this definition, "submitted"
-      means any form of electronic, verbal, or written communication sent
-      to the Licensor or its representatives, including but not limited to
-      communication on electronic mailing lists, source code control systems,
-      and issue tracking systems that are managed by, or on behalf of, the
-      Licensor for the purpose of discussing and improving the Work, but
-      excluding communication that is conspicuously marked or otherwise
-      designated in writing by the copyright owner as "Not a Contribution."
-
-      "Contributor" shall mean Licensor and any individual or Legal Entity
-      on behalf of whom a Contribution has been received by Licensor and
-      subsequently incorporated within the Work.
-
-   2. Grant of Copyright License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      copyright license to reproduce, prepare Derivative Works of,
-      publicly display, publicly perform, sublicense, and distribute the
-      Work and such Derivative Works in Source or Object form.
-
-   3. Grant of Patent License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      (except as stated in this section) patent license to make, have made,
-      use, offer to sell, sell, import, and otherwise transfer the Work,
-      where such license applies only to those patent claims licensable
-      by such Contributor that are necessarily infringed by their
-      Contribution(s) alone or by combination of their Contribution(s)
-      with the Work to which such Contribution(s) was submitted. If You
-      institute patent litigation against any entity (including a
-      cross-claim or counterclaim in a lawsuit) alleging that the Work
-      or a Contribution incorporated within the Work constitutes direct
-      or contributory patent infringement, then any patent licenses
-      granted to You under this License for that Work shall terminate
-      as of the date such litigation is filed.
-
-   4. Redistribution. You may reproduce and distribute copies of the
-      Work or Derivative Works thereof in any medium, with or without
-      modifications, and in Source or Object form, provided that You
-      meet the following conditions:
-
-      (a) You must give any other recipients of the Work or
-          Derivative Works a copy of this License; and
-
-      (b) You must cause any modified files to carry prominent notices
-          stating that You changed the files; and
-
-      (c) You must retain, in the Source form of any Derivative Works
-          that You distribute, all copyright, patent, trademark, and
-          attribution notices from the Source form of the Work,
-          excluding those notices that do not pertain to any part of
-          the Derivative Works; and
-
-      (d) If the Work includes a "NOTICE" text file as part of its
-          distribution, then any Derivative Works that You distribute must
-          include a readable copy of the attribution notices contained
-          within such NOTICE file, excluding those notices that do not
-          pertain to any part of the Derivative Works, in at least one
-          of the following places: within a NOTICE text file distributed
-          as part of the Derivative Works; within the Source form or
-          documentation, if provided along with the Derivative Works; or,
-          within a display generated by the Derivative Works, if and
-          wherever such third-party notices normally appear. The contents
-          of the NOTICE file are for informational purposes only and
-          do not modify the License. You may add Your own attribution
-          notices within Derivative Works that You distribute, alongside
-          or as an addendum to the NOTICE text from the Work, provided
-          that such additional attribution notices cannot be construed
-          as modifying the License.
-
-      You may add Your own copyright statement to Your modifications and
-      may provide additional or different license terms and conditions
-      for use, reproduction, or distribution of Your modifications, or
-      for any such Derivative Works as a whole, provided Your use,
-      reproduction, and distribution of the Work otherwise complies with
-      the conditions stated in this License.
-
-   5. Submission of Contributions. Unless You explicitly state otherwise,
-      any Contribution intentionally submitted for inclusion in the Work
-      by You to the Licensor shall be under the terms and conditions of
-      this License, without any additional terms or conditions.
-      Notwithstanding the above, nothing herein shall supersede or modify
-      the terms of any separate license agreement you may have executed
-      with Licensor regarding such Contributions.
-
-   6. Trademarks. This License does not grant permission to use the trade
-      names, trademarks, service marks, or product names of the Licensor,
-      except as required for reasonable and customary use in describing the
-      origin of the Work and reproducing the content of the NOTICE file.
-
-   7. Disclaimer of Warranty. Unless required by applicable law or
-      agreed to in writing, Licensor provides the Work (and each
-      Contributor provides its Contributions) on an "AS IS" BASIS,
-      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
-      implied, including, without limitation, any warranties or conditions
-      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
-      PARTICULAR PURPOSE. You are solely responsible for determining the
-      appropriateness of using or redistributing the Work and assume any
-      risks associated with Your exercise of permissions under this License.
-
-   8. Limitation of Liability. In no event and under no legal theory,
-      whether in tort (including negligence), contract, or otherwise,
-      unless required by applicable law (such as deliberate and grossly
-      negligent acts) or agreed to in writing, shall any Contributor be
-      liable to You for damages, including any direct, indirect, special,
-      incidental, or consequential damages of any character arising as a
-      result of this License or out of the use or inability to use the
-      Work (including but not limited to damages for loss of goodwill,
-      work stoppage, computer failure or malfunction, or any and all
-      other commercial damages or losses), even if such Contributor
-      has been advised of the possibility of such damages.
-
-   9. Accepting Warranty or Additional Liability. While redistributing
-      the Work or Derivative Works thereof, You may choose to offer,
-      and charge a fee for, acceptance of support, warranty, indemnity,
-      or other liability obligations and/or rights consistent with this
-      License. However, in accepting such obligations, You may act only
-      on Your own behalf and on Your sole responsibility, not on behalf
-      of any other Contributor, and only if You agree to indemnify,
-      defend, and hold each Contributor harmless for any liability
-      incurred by, or claims asserted against, such Contributor by reason
-      of your accepting any such warranty or additional liability.
-
-   END OF TERMS AND CONDITIONS
-
-   APPENDIX: How to apply the Apache License to your work.
-
-      To apply the Apache License to your work, attach the following
-      boilerplate notice, with the fields enclosed by brackets "[]"
-      replaced with your own identifying information. (Don't include
-      the brackets!)  The text should be enclosed in the appropriate
-      comment syntax for the file format. We also recommend that a
-      file or class name and description of purpose be included on the
-      same "printed page" as the copyright notice for easier
-      identification within third-party archives.
-
-   Copyright [yyyy] [name of copyright owner]
-
-   Licensed under the Apache License, Version 2.0 (the "License");
-   you may not use this file except in compliance with the License.
-   You may obtain a copy of the License at
-
-       http://www.apache.org/licenses/LICENSE-2.0
-
-   Unless required by applicable law or agreed to in writing, software
-   distributed under the License is distributed on an "AS IS" BASIS,
-   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-   See the License for the specific language governing permissions and
-   limitations under the License.

toid_vision/config/camera_info.yaml

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

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

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

@@ -1,26 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>toid_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

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

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