.clang-format

@@ -0,0 +1,19 @@
+---
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+BraceWrapping:
+  AfterClass: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+  AfterEnum: true
+BreakBeforeBraces: Custom
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+ReflowComments: false

.gitignore

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

.gitmodules

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

Dockerfile

@@ -1,15 +1,33 @@
-FROM ros:jazzy-ros-base
+FROM ros:jazzy-perception
 
 ENV DEBIAN_FRONTEND=noninteractive
 
 # ---------- System dependencies ----------
-RUN apt-get update && apt-get install -y \
+RUN apt-get update && apt-get upgrade && apt-get install -y \
     python3-colcon-common-extensions \
     python3-rosdep \
+    meson cmake \
     build-essential \
     udev \
     git
 
+# ---------- Libcamera ----------
+
+WORKDIR /extras
+
+RUN apt-get install -y libboost-dev \
+    libgnutls28-dev openssl libtiff5-dev \
+    pybind11-dev \
+    qtbase5-dev libqt5core5a libqt5gui5 libqt5widgets5 \
+    python3-yaml python3-ply python3-jinja2 python3-pip\
+    libglib2.0-dev libgstreamer-plugins-base1.0-dev \
+    build-essential
+
+RUN git clone https://github.com/raspberrypi/libcamera.git \
+    && cd libcamera \
+    && meson setup build --buildtype=release -Dpipelines=rpi/vc4,rpi/pisp -Dipas=rpi/vc4,rpi/pisp -Dv4l2=true -Dgstreamer=enabled -Dtest=false -Dlc-compliance=disabled -Dcam=disabled -Dqcam=disabled -Ddocumentation=disabled -Dpycamera=enabled \
+    && ninja -C build install
+
 # ---------- Initialize rosdep ----------
 RUN rosdep init || true
 RUN rosdep update
@@ -18,25 +36,33 @@ RUN rosdep update
 WORKDIR /ros_ws
 
 # ---------- Copy package.xml files ----------
-COPY toid_bot_description/package.xml toid_bot_description/package.xml
+COPY toid_bot_description/package.xml    toid_bot_description/package.xml
-COPY toid_control/package.xml         toid_control/package.xml
+COPY toid_control/package.xml            toid_control/package.xml
-COPY toid_msgs/package.xml            toid_msgs/package.xml
+COPY toid_msgs/package.xml               toid_msgs/package.xml
-COPY toid_odometry/package.xml        toid_odometry/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_navigation/package.xml      toid_navigation/package.xml
+COPY toid_behaviors/package.xml          toid_behaviors/package.xml
+COPY toid_navigation/package.xml         toid_navigation/package.xml
+COPY toid_vision/package.xml             toid_vision/package.xml
+COPY ext/camera_ros/package.xml          ext/camera_ros/package.xml
+COPY ext/BehaviorTree.ROS2/              ext/BehaviorTree.ROS2/
 
 # ---------- Install dependencies ----------
 RUN . /opt/ros/jazzy/setup.sh && \
-    rosdep install --from-paths ./ --ignore-src -r -y \
+    rosdep install --from-paths ./ --ignore-src -r -y --skip-keys=libcamera \
     && rm -rf /var/lib/apt/lists/*
 
 RUN rm -rf ./*
 
 RUN cat <<EOF >> /root/.bashrc
 source /opt/ros/jazzy/setup.bash
-if [[-f ./install/setup.bash ]]
+if [[ -f ./install/setup.bash ]]; then
     source ./install/setup.bash
-if
+fi
 EOF
 
 

docker-compose.yaml

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

firmware/base/src/config.h

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

firmware/base/src/main.c

@@ -62,8 +62,8 @@ typedef struct calib_diff_t {
 } calib_diff_t;
 
 static calib_diff_t calib_enc = {
-    .left_gain = 1.000,
+    .left_gain = 1.0,  
-    .right_gain = 1.0000
+    .right_gain = 1.0,
 };
 
 void update_pos_cb() {

firmware/base/src/msgs.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <pico/stdio.h>
 #include "crc.h"
 #include "stdio.h"
 
@@ -58,15 +59,22 @@ enum {
     TSTATE_BUILD_MSG,
 };
 
-inline void send_tRespState(tRespState *resp) {
+static inline void write_raw(void* data, size_t n) {
+    const uint8_t* a = data;
+    for(size_t i = 0; i < n; i++){
+        putchar_raw(a[i]);
+    }
+}
+
+static inline void send_tRespState(tRespState *resp) {
     resp->crc = crcFooter(*resp);
-    fwrite(resp, 1, sizeof(tRespState), stdout);
+    write_raw(resp, sizeof(tRespState));
     fflush(stdout);
 }
 
-inline void send_tRespEndCalib(tRespEndCalib *state) {
+static inline void send_tRespEndCalib(tRespEndCalib *state) {
     state->crc = crcFooter(*state);
-    fwrite(state, 1, sizeof(tRespEndCalib), stdout);
+    write_raw(state, sizeof(tRespEndCalib));
     fflush(stdout);
 }
 

firmware/base/tusb/tusb_stdio_driver.c

@@ -51,7 +51,7 @@ static void stdio_dual_usb_out_chars(const char *buf, int length) {
         tud_task();
         tud_cdc_n_write_flush(itf);
         if (!stdio_dual_usb_connected() ||
-            (!tud_cdc_write_available() &&
+            (!tud_cdc_n_write_available(itf) &&
              time_us_64() >
                  last_avail_time[itf] + STDIO_DUAL_USB_STDOUT_TIMEOUT_US)) {
           break;

scripts/idi.sh

@@ -0,0 +1,54 @@
+#!/bin/bash
+
+TARGET="$1"
+
+if [[ "$TARGET" == "-h" || "$TARGET" == "--help" ]]; then
+    exit 0
+fi
+
+if [[ $# -lt 1 ]]; then
+    echo "Error: Expected at least 1 arg"
+    echo "Available commands: napred, rotiraj, na"
+    exit 1
+fi
+
+if [[ $# -ge 2 ]]; then
+  SMER="$2"
+fi
+
+if [[ $# -ge 3 ]]; then
+  MIN_ROT="$(echo "$3*a(1)/45" | bc -l)"
+fi
+
+source install/setup.bash
+
+case $TARGET in
+    "na") 
+        ros2 action send_goal /moveCoords toid_msgs/action/SimpleMoveCoords "
+          x: 0.0
+          y: 0.0
+          theta: 0.0
+          backwards: ${SMER:-0}"
+        ;;
+    "pravo") 
+        ros2 action send_goal /translateX toid_msgs/action/SimpleTranslateX  "distance: ${SMER:-0.3}"
+        ;;
+    "rotiraj") 
+        SMER="$(echo "$SMER*a(1)/45" | bc -l)"
+        echo $SMER
+        ros2 action send_goal /rotate toid_msgs/action/SimpleRotate  "
+          angle: ${SMER:-0.3}
+          min_angle: ${MIN_ROT:-0.0}"
+        ;;
+    "stani") 
+        ros2 service call /moveCoords/_action/cancel_goal action_msgs/srv/CancelGoal &
+        ros2 service call /translateX/_action/cancel_goal action_msgs/srv/CancelGoal &
+        ros2 service call /rotate/_action/cancel_goal action_msgs/srv/CancelGoal &
+        wait
+        ;;
+    *)
+        echo "Target not defined"
+        exit 1
+        ;;
+esac
+exit 0

scripts/toid_base_build.sh

@@ -0,0 +1,6 @@
+#!/bin/env bash
+
+source /opt/ros/jazzy/setup.bash
+
+colcon build --packages-up-to-regex "toid_(?!bt)"
+colcon build --packages-up-to-regex "camera_ros"

toid_behaviors/CMakeLists.txt

@@ -0,0 +1,87 @@
+cmake_minimum_required(VERSION 3.8)
+project(toid_behaviors)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(library_name toid_behaviors)
+
+set(
+  PACKAGE_DEPS
+
+  rclcpp
+  angles
+  geometry_msgs
+  pluginlib
+  nav_msgs
+  nav2_core
+  nav2_behaviors
+  nav2_costmap_2d
+  nav2_util
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+  toid_msgs
+)
+
+set(
+  SOURCES
+  src/scl.cpp
+  src/move_coords.cpp
+  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)
+foreach(PACKAGE ${PACKAGE_DEPS})
+  find_package(${PACKAGE} REQUIRED)
+endforeach()
+
+add_library(${library_name} SHARED ${SOURCES})
+
+target_include_directories(
+  ${library_name} 
+  PRIVATE 
+
+  include
+)
+
+ament_target_dependencies(
+  ${library_name}
+  ${PACKAGE_DEPS}
+)
+
+install(TARGETS ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin 
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${PACKAGE_DEPS})
+
+pluginlib_export_plugin_description_file(nav2_core toid_behaviors.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

toid_behaviors/LICENSE

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

toid_behaviors/include/toid_behaviors/approach_acorns.hpp

@@ -0,0 +1,91 @@
+#pragma once
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "toid_behaviors/scl.hpp"
+#include "toid_behaviors/simple_move.hpp"
+#include "toid_behaviors/rolling_average.hpp"
+#include "toid_msgs/action/simple_move_coords.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+
+namespace toid
+{
+using MoveAction = toid_msgs::action::SimpleMoveCoords;
+using PoseStamped = geometry_msgs::msg::PoseStamped;
+using namespace nav2_behaviors;
+
+class ApproachAcorns : public SimpleMove<MoveAction>
+{
+public:
+  ApproachAcorns();
+  ~ApproachAcorns();
+
+  void configureCB() override;
+
+  void activateCB() override;
+  void deactivateCB() override;
+
+  ResultStatus onStart(
+    const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+    const geometry_msgs::msg::Twist & vel) override;
+
+  double distanceToTarget(
+    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point,
+    const double target_theta, bool backwards);
+
+  double velocityTarget(const double dist_left);
+
+  bool collisionDetection(
+    const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose);
+
+  void acorn_position_cb(PoseStamped::ConstSharedPtr msg);
+
+  ResultStatus updateVel(
+    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
+    geometry_msgs::msg::Twist & out_vel) override;
+
+  virtual nav2_core::CostmapInfoType getResourceInfo() override
+  {
+    return nav2_core::CostmapInfoType::LOCAL;
+  }
+
+protected:
+  SmoothControlLaw scl_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr acorn_pose_sub_;
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr target_pose_pub_;
+  std::shared_mutex mutex_;
+  RollingAverage avg_ = RollingAverage(10);
+
+  //Goal
+  geometry_msgs::msg::Pose new_target_pose_;
+  double new_target_angle_;
+
+  geometry_msgs::msg::Pose target_pose_;
+  geometry_msgs::msg::Pose initial_pose_;
+  double target_angle_;
+  double target_sign_;
+  bool backwards_;
+  unsigned char mode_;
+
+  //State
+  double last_speed_;
+  rclcpp::Time last_seen_;
+  double distance_;
+
+  //Config
+  double max_vel_accel_;
+  double max_vel_decel_;
+  double max_vel_speed_;
+  double min_vel_speed_;
+
+  double max_angular_speed_;
+
+  double kp_;
+  double k_phi_;
+  double k_delta_;
+  double beta_;
+  double lambda_;
+  double slowdown_radius_;
+  bool debug_marker_;
+};
+
+}  // namespace toid

toid_behaviors/include/toid_behaviors/move_coords.hpp

@@ -0,0 +1,72 @@
+#pragma once
+
+#include "toid_behaviors/simple_move.hpp"
+#include "toid_behaviors/scl.hpp"
+#include "toid_msgs/action/simple_move_coords.hpp"
+
+#include "geometry_msgs/msg/pose.hpp"
+
+namespace toid
+{
+using MoveAction = toid_msgs::action::SimpleMoveCoords;
+using namespace nav2_behaviors;
+
+class MoveCoords : public SimpleMove<MoveAction>
+{
+public:
+  MoveCoords();
+  ~MoveCoords();
+
+  void configureCB() 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);
+
+  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_;
+
+  //Goal
+  geometry_msgs::msg::Pose target_pose_;
+  double target_angle_;
+  double target_sign_;
+  bool backwards_;
+  unsigned char mode_;
+
+  //State
+  double last_speed_;
+
+  //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_;
+};
+
+}  // namespace toid
+

toid_behaviors/include/toid_behaviors/rolling_average.hpp

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

toid_behaviors/include/toid_behaviors/rotate_acorns.hpp

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

toid_behaviors/include/toid_behaviors/scl.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+namespace toid
+{
+class SmoothControlLaw
+{
+public:
+  double k_phi = 5.0;
+  double k_delta = 2.0;
+  double bbeta = 0.4;
+  double lam = 2.0;
+
+  double slowdown_radius = 0.20;
+  double v_linear_min = 0.05;
+  double v_linear_max = 0.1;
+  double v_angular_max = 2.0;
+
+  void egocentric_polar(
+    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
+    bool backwards, double & r, double & phi, double & delta);
+
+  double curvature(double r, double phi, double delta);
+
+  void calculate_vel(
+    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
+    geometry_msgs::msg::Twist & out_speed, bool backwards = false);
+
+  void step(
+    const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt,
+    bool backwards = false);
+};
+}  // namespace toid

toid_behaviors/include/toid_behaviors/simple_move.hpp

@@ -0,0 +1,179 @@
+#pragma once
+#include "nav2_behaviors/timed_behavior.hpp"
+#include "nav2_core/behavior.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/twist_publisher.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "toid_msgs/action/simple_rotate.hpp"
+#include "toid_msgs/msg/rival.hpp"
+
+namespace toid
+{
+
+template <typename ActionT>
+class SimpleMove : public nav2_behaviors::TimedBehavior<ActionT>
+{
+public:
+  using Rival = toid_msgs::msg::Rival;
+
+  virtual void configureCB() {}
+  virtual void cleanupCB() {}
+
+  virtual void activateCB() {}
+  virtual void deactivateCB() {}
+
+  virtual nav2_behaviors::ResultStatus onStart(
+    const std::shared_ptr<const typename ActionT::Goal> command,
+    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel) = 0;
+
+  virtual nav2_behaviors::ResultStatus updateVel(
+    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
+    geometry_msgs::msg::Twist & out_vel) = 0;
+
+  void onConfigure() override
+  {
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock();
+    nav2_util::declare_parameter_if_not_declared(
+      node, "odom_topic", rclcpp::ParameterValue("/odom"));
+    std::string odom_topic_name = node->get_parameter("odom_topic").as_string();
+
+    nav2_util::declare_parameter_if_not_declared(node, "robot_width", rclcpp::ParameterValue(0.30));
+    node->get_parameter("robot_width", robot_width_);
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, "robot_length", rclcpp::ParameterValue(0.30));
+    node->get_parameter("robot_length", robot_length_);
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, "rival_radius", rclcpp::ParameterValue(0.30));
+    node->get_parameter("rival_radius", rival_radius_);
+
+    odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
+      odom_topic_name, 1, [&](nav_msgs::msg::Odometry msg) {
+        std::lock_guard lock(mut_);
+        current_vel_ = msg.twist.twist;
+      });
+    control_duration_ = 1.0 / this->cycle_frequency_;
+    configureCB();
+  }
+
+  void onCleanup() override
+  {
+    odom_sub_.reset();
+    cleanupCB();
+  }
+
+  void activate() override
+  {
+    nav2_behaviors::TimedBehavior<ActionT>::activate();
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock();
+    using namespace std::placeholders;
+    rivals_sub_ = node->create_subscription<Rival>(
+      "/dynamicObstacle", 1, std::bind(&SimpleMove<ActionT>::rival_cb, this, _1));
+    activateCB();
+  }
+
+  void deactivate() override
+  {
+    nav2_behaviors::TimedBehavior<ActionT>::deactivate();
+    rivals_sub_.reset();
+    deactivateCB();
+  }
+
+  nav2_behaviors::ResultStatus onRun(
+    const std::shared_ptr<const typename ActionT::Goal> command) override
+  {
+    geometry_msgs::msg::PoseStamped pose;
+    geometry_msgs::msg::Twist vel;
+    {
+      std::lock_guard lock(mut_);
+      vel = current_vel_;
+    }
+
+    if (!nav2_util::getCurrentPose(
+          pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
+          this->transform_tolerance_)) {
+      return nav2_behaviors::ResultStatus{nav2_behaviors::Status::FAILED};
+    }
+
+    return onStart(command, pose.pose, vel);
+  }
+
+  nav2_behaviors::ResultStatus onCycleUpdate() override
+  {
+    geometry_msgs::msg::PoseStamped pose;
+    geometry_msgs::msg::Twist vel;
+    geometry_msgs::msg::Twist out_vel;
+    auto vel_p = std::make_unique<geometry_msgs::msg::TwistStamped>();
+    {
+      std::lock_guard lock(mut_);
+      vel = current_vel_;
+    }
+
+    if (!nav2_util::getCurrentPose(
+          pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
+          this->transform_tolerance_)) {
+      return nav2_behaviors::ResultStatus{nav2_behaviors::Status::FAILED};
+    }
+
+    nav2_behaviors::ResultStatus r = updateVel(pose.pose, vel, out_vel);
+    vel_p->twist = out_vel;
+    vel_p->header.stamp = this->clock_->now();
+    vel_p->header.frame_id = this->robot_base_frame_;
+    this->vel_pub_->publish(std::move(vel_p));
+    return r;
+  }
+
+  bool check_rival_collision(geometry_msgs::msg::Pose2D & pose)
+  {
+    if (!rivals_ || (this->clock_->now() - rivals_->header.stamp ) > rclcpp::Duration::from_seconds(1.0)) {
+      return false;
+    }
+    const double cosp = std::cos(pose.theta);
+    const double sinp = std::sin(pose.theta);
+    int i = 0;
+    for (const auto & rival : rivals_->point) {
+      geometry_msgs::msg::Point local_rival;
+      const double dx = rival.x - pose.x;
+      const double dy = rival.y - pose.y;
+      local_rival.x = dx * cosp - dy * sinp;
+      local_rival.y = dx * sinp + dy * cosp;
+
+      local_rival.x -= 0.105;
+
+      const double qx = std::abs(local_rival.x) - robot_length_ / 2.0;
+      const double qy = std::abs(local_rival.y) - robot_width_ / 2.0;
+
+      const double mqx = std::max(qx, 0.0);
+      const double mqy = std::max(qy, 0.0);
+
+      double length = std::sqrt(mqx * mqx + mqy * mqy);
+
+      double sdf = length + std::min(std::max(qx, qy), 0.0);
+      RCLCPP_DEBUG(this->logger_, "Distance to rival %d: %lf", i++, sdf-rival_radius_);
+      if (sdf < rival_radius_) {
+        return true;
+      }
+    }
+    return false;
+  }
+
+  void rival_cb(Rival::SharedPtr msg) { rivals_ = msg; }
+
+protected:
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  geometry_msgs::msg::Pose current_pose_;
+  geometry_msgs::msg::Twist current_vel_;
+  std::recursive_mutex mut_;
+  double control_duration_;
+
+  double robot_width_ = 0.30;
+  double robot_length_ = 0.30;
+  double rival_radius_ = 0.22;
+
+  Rival::SharedPtr rivals_;
+  rclcpp::Subscription<Rival>::SharedPtr rivals_sub_;
+};
+
+}  // namespace toid

toid_behaviors/include/toid_behaviors/simple_rotate.hpp

@@ -0,0 +1,60 @@
+#pragma once
+
+#include "toid_behaviors/simple_move.hpp"
+#include "toid_msgs/action/simple_rotate.h"
+#include "toid_msgs/msg/rival.hpp"
+
+namespace toid
+{
+using RotateAction = toid_msgs::action::SimpleRotate;
+using namespace nav2_behaviors;
+
+class SimpleRotateBehavior : public SimpleMove<RotateAction>
+{
+public:
+  SimpleRotateBehavior();
+  ~SimpleRotateBehavior();
+
+  void configureCB() override;
+
+  ResultStatus onStart(
+    const std::shared_ptr<const RotateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+    const geometry_msgs::msg::Twist & vel) override;
+
+  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:
+  //Goal
+  double target_angle_;
+  double min_turn_angle_;
+  double initial_direction_;
+  unsigned char mode_;
+
+  //State
+  double angular_speed_;
+  double last_angle_;
+  double last_time_;
+
+  //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_translate_x.hpp

@@ -0,0 +1,53 @@
+#pragma once
+
+#include "toid_behaviors/simple_move.hpp"
+#include "toid_msgs/action/simple_translate_x.hpp"
+
+namespace toid
+{
+using TranslateAction = toid_msgs::action::SimpleTranslateX;
+using namespace nav2_behaviors;
+
+class SimpleTranslateXBehavior : public SimpleMove<TranslateAction>
+{
+public:
+  SimpleTranslateXBehavior();
+  ~SimpleTranslateXBehavior();
+
+  void configureCB() override;
+
+  ResultStatus onStart(
+    const std::shared_ptr<const TranslateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+    const geometry_msgs::msg::Twist & vel) override;
+
+  ResultStatus updateVel(
+    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
+    geometry_msgs::msg::Twist & out_vel) override;
+  
+  virtual nav2_core::CostmapInfoType getResourceInfo() override {
+    return nav2_core::CostmapInfoType::NONE;
+  }
+
+protected:
+
+  //Goal
+  double target_distance_;
+  double target_angle_;
+  double target_sign_;
+  geometry_msgs::msg::Pose initial_pose_;
+
+  //State
+  double speed_;
+
+  //Config
+  double max_vel_accel_;
+  double max_vel_decel_;
+  double max_vel_speed_;
+  double min_vel_speed_;
+  double max_angular_speed_;
+
+  double kp_;
+};
+
+}  // namespace toid
+

toid_behaviors/package.xml

@@ -0,0 +1,31 @@
+<?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_behaviors</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="82343504+pimpest@users.noreply.github.com">Pimpest</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>angles</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_core</depend>
+  <depend>nav2_behaviors</depend>
+  <depend>pluginlib</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>toid_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

toid_behaviors/src/approach_acorns.cpp

@@ -0,0 +1,311 @@
+#include "toid_behaviors/approach_acorns.hpp"
+
+#include "angles/angles.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+ApproachAcorns::ApproachAcorns() : SimpleMove<MoveAction>() {}
+ApproachAcorns::~ApproachAcorns() {}
+
+void ApproachAcorns::configureCB()
+{
+  auto node = node_.lock();
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_vel_accel", rclcpp::ParameterValue(2.0));
+  node->get_parameter(behavior_name_ + ".max_vel_accel", max_vel_accel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_vel_decel", rclcpp::ParameterValue(1.0));
+  node->get_parameter(behavior_name_ + ".max_vel_decel", max_vel_decel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_vel_speed", rclcpp::ParameterValue(0.4));
+  node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".min_vel_speed", rclcpp::ParameterValue(0.10));
+  node->get_parameter(behavior_name_ + ".min_vel_speed", min_vel_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_speed", rclcpp::ParameterValue(1.0));
+  node->get_parameter(behavior_name_ + ".max_angular_speed", max_angular_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".kp", rclcpp::ParameterValue(5.0));
+  node->get_parameter(behavior_name_ + ".kp", kp_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".kphi", rclcpp::ParameterValue(5.0));
+  node->get_parameter(behavior_name_ + ".kphi", k_phi_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".kdelta", rclcpp::ParameterValue(2.0));
+  node->get_parameter(behavior_name_ + ".kdelta", k_delta_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".beta", rclcpp::ParameterValue(0.4));
+  node->get_parameter(behavior_name_ + ".beta", beta_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".lambda", rclcpp::ParameterValue(2.0));
+  node->get_parameter(behavior_name_ + ".lambda", lambda_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".slow_down_radius", rclcpp::ParameterValue(0.20));
+  node->get_parameter(behavior_name_ + ".slow_down_radius", slowdown_radius_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".debug_marker", rclcpp::ParameterValue(false));
+  node->get_parameter(behavior_name_ + ".debug_marker", debug_marker_);
+
+  target_pose_pub_ = node->create_publisher<visualization_msgs::msg::Marker>("marker", 1);
+}
+
+void ApproachAcorns::activateCB()
+{
+  auto node = node_.lock();
+  using namespace std::placeholders;
+  acorn_pose_sub_ = node->create_subscription<PoseStamped>(
+    "closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&ApproachAcorns::acorn_position_cb, this, _1));
+  target_pose_pub_->on_activate();
+  distance_ = 1.0;
+}
+
+void ApproachAcorns::deactivateCB()
+{
+  acorn_pose_sub_.reset();
+  target_pose_pub_->on_deactivate();
+}
+
+void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
+{
+  geometry_msgs::msg::PoseStamped pose_local;
+  geometry_msgs::msg::PoseStamped pose_global;
+  try {
+    pose_local = tf_->transform(*msg, robot_base_frame_);
+    pose_global = tf_->transform(*msg, global_frame_);
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]...");
+    return;
+  }
+
+  double x = pose_local.pose.position.x;
+  double y = pose_local.pose.position.y;
+  if (std::sqrt(x * x + y * y) > distance_ + 0.005) {
+    return;
+  }
+
+  // Smooth out approach
+  if (x * x + y * y < 0.42 * 0.42) {
+    return;
+  }
+
+  double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation));
+
+  if (tf2::getYaw(pose_local.pose.orientation) > 0) {
+    pose_global.pose.position.x += std::cos(yaw) * 0.01 - std::sin(yaw) * 0.35;
+    pose_global.pose.position.y += std::sin(yaw) * 0.01 + std::cos(yaw) * 0.35;
+  } else {
+    pose_global.pose.position.x += std::cos(yaw) * -0.01 - std::sin(yaw) * -0.35;
+    pose_global.pose.position.y += std::sin(yaw) * -0.01 + std::cos(yaw) * -0.35;
+  }
+
+  std::lock_guard _lock(mutex_);
+
+  const double dx = pose_global.pose.position.x - initial_pose_.position.x;
+  const double dy = pose_global.pose.position.y - initial_pose_.position.y;
+  double yaw_to_goal = std::atan2(dy,dx);
+
+  tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw_to_goal), pose_global.pose.orientation);
+  yaw = yaw_to_goal;
+  auto [a, b, c] = avg_.push(pose_global.pose);
+  distance_ = x * x + y * y;
+  new_target_pose_.position.x = a;
+  new_target_pose_.position.y = b;
+  tf2::convert(tf2::Quaternion::createFromRPY(0, 0, c), new_target_pose_.orientation);
+  new_target_angle_ = c;
+
+  if (debug_marker_) {
+    visualization_msgs::msg::Marker marker;
+    marker.lifetime.sec = 1.0;
+    marker.header = pose_global.header;
+    marker.pose = new_target_pose_;
+    marker.type = visualization_msgs::msg::Marker::ARROW;
+    marker.scale.x = 0.02;
+    marker.scale.y = 0.02;
+    marker.scale.z = 0.02;
+    marker.color.a = 1.0;
+    marker.color.r = 1.0;
+    marker.color.g = 0;
+    marker.color.b = 0;
+    target_pose_pub_->publish(marker);
+  }
+}
+
+ResultStatus ApproachAcorns::onStart(
+  const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+  const geometry_msgs::msg::Twist & vel)
+{
+  std::lock_guard _lock(mutex_);
+  distance_ = 1.0;
+  new_target_pose_.position.x = command->x;
+  new_target_pose_.position.y = command->y;
+
+  backwards_ = command->backwards;
+  initial_pose_ = pose;
+
+  new_target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta));
+  new_target_angle_ = command->theta;
+  target_sign_ = backwards_ ? -1.0 : 1.0;
+  max_vel_speed_ = command->max_speed;
+
+  avg_.reset();
+
+  if (command->max_speed == 0) {
+    auto node = node_.lock();
+    node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
+  }
+
+  mode_ = command->mode;
+
+  scl_.k_phi = k_phi_;
+  scl_.k_delta = k_delta_;
+  scl_.bbeta = beta_;
+  scl_.lam = lambda_;
+  scl_.slowdown_radius = slowdown_radius_;
+  scl_.v_angular_max = max_angular_speed_;
+  scl_.v_linear_min = min_vel_speed_;
+  scl_.v_linear_max = max_vel_speed_;
+
+  last_speed_ = vel.angular.x;
+
+  return ResultStatus{Status::SUCCEEDED};
+}
+
+double ApproachAcorns::distanceToTarget(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point,
+  const double target_theta, bool backwards)
+{
+  const double dx = target_point.x - pose.position.x;
+  const double dy = target_point.y - pose.position.y;
+  const double target_sign = backwards ? -1.0 : 1.0;
+
+  return target_sign * (dx * cos(target_theta) + dy * sin(target_theta));
+}
+
+double ApproachAcorns::velocityTarget(const double dist_left)
+{
+  const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_;
+  const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_;
+
+  double vel = max_vel_speed_;
+  double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
+  vel = std::min(vel, max_vel_to_stop);
+  return std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+}
+
+bool ApproachAcorns::collisionDetection(
+  const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose)
+{
+  const int samples = static_cast<int>(0.5 / control_duration_);
+  geometry_msgs::msg::Pose current_pose = pose;
+  last_ok_pose = pose;
+  const bool check_map = !(mode_ & MoveAction::Goal::IGNORE_OBSTACLES);
+  for (int i = 0; i < samples; i++) {
+    scl_.step(target_pose_, current_pose, control_duration_, backwards_);
+
+    geometry_msgs::msg::Pose2D p;
+    p.x = current_pose.position.x;
+    p.y = current_pose.position.y;
+    p.theta = tf2::getYaw(current_pose.orientation);
+
+    if (check_map && !local_collision_checker_->isCollisionFree(p, i == 0)) {
+      return true;
+    }
+
+    if (check_rival_collision(p)) {
+      return true;
+    }
+
+    last_ok_pose = current_pose;
+    const double dist_left =
+      distanceToTarget(current_pose, target_pose_.position, target_angle_, backwards_);
+    if (dist_left < 0.005) {
+      return false;
+    }
+  }
+  return false;
+}
+
+ResultStatus ApproachAcorns::updateVel(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
+  geometry_msgs::msg::Twist & out_vel)
+{
+  {
+    std::lock_guard _lock(mutex_);
+    target_pose_ = new_target_pose_;
+    target_angle_ = new_target_angle_;
+  }
+
+  double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_);
+
+  const double current_yaw = tf2::getYaw(pose.orientation);
+  double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
+
+  if (dist_left <= 0.001 && std::fabs(angle_dist) <= 0.030) {
+    out_vel.linear.x = 0;
+    out_vel.angular.z = 0;
+    return ResultStatus{Status::SUCCEEDED};
+  }
+
+  geometry_msgs::msg::Pose last_ok_pose;
+  if (collisionDetection(pose, last_ok_pose)) {
+    dist_left = distanceToTarget(
+      pose, last_ok_pose.position, tf2::getYaw(last_ok_pose.orientation), backwards_);
+    if (dist_left <= 0.02) {
+      out_vel.linear.x = 0;
+      out_vel.angular.z = 0;
+    } else {
+      scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
+      scl_.calculate_vel(last_ok_pose, pose, out_vel, backwards_);
+    }
+
+    last_speed_ = out_vel.linear.x;
+
+    RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
+    RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max);
+
+    return ResultStatus{Status::RUNNING};
+  }
+
+  if (dist_left >= 0.001 && dist_left <= 0.02) {
+    out_vel.linear.x = velocityTarget(dist_left);
+    out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
+    last_speed_ = out_vel.linear.x;
+    return ResultStatus{Status::RUNNING};
+  }
+  
+  if (dist_left <= 0.001) {
+    out_vel.linear.x = 0;
+    out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
+    last_speed_ = out_vel.linear.x;
+    return ResultStatus{Status::RUNNING};
+  }
+
+  scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
+  scl_.calculate_vel(target_pose_, pose, out_vel, backwards_);
+
+  last_speed_ = out_vel.linear.x;
+
+  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
+  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max);
+
+  return ResultStatus{Status::RUNNING};
+}
+
+}  // namespace toid
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::ApproachAcorns, nav2_core::Behavior);

toid_behaviors/src/move_coords.cpp

@@ -0,0 +1,204 @@
+#include "toid_behaviors/move_coords.hpp"
+
+#include "angles/angles.h"
+
+namespace toid
+{
+
+MoveCoords::MoveCoords() : SimpleMove<MoveAction>() {}
+MoveCoords::~MoveCoords() {}
+
+void MoveCoords::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_);
+}
+
+ResultStatus MoveCoords::onStart(
+  const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose &,
+  const geometry_msgs::msg::Twist & vel)
+{
+  target_pose_.position.x = command->x;
+  target_pose_.position.y = command->y;
+  backwards_ = command->backwards;
+
+  target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta));
+  target_angle_ = command->theta;
+  target_sign_ = backwards_ ? -1.0 : 1.0;
+  max_vel_speed_ = command->max_speed;
+
+  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 MoveCoords::distanceToTarget(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point,
+  const double target_theta, bool backwards)
+{
+  const double dx = target_point.x - pose.position.x;
+  const double dy = target_point.y - pose.position.y;
+  const double target_sign = backwards ? -1.0 : 1.0;
+
+  return target_sign * (dx * cos(target_theta) + dy * sin(target_theta));
+}
+
+double MoveCoords::velocityTarget(const double dist_left)
+{
+  const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_;
+  const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_;
+
+  double vel = max_vel_speed_;
+  double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
+  vel = std::min(vel, max_vel_to_stop);
+  return std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+}
+
+bool MoveCoords::collisionDetection(
+  const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose)
+{
+  const int samples = static_cast<int>(0.5 / control_duration_);
+  geometry_msgs::msg::Pose current_pose = pose;
+  last_ok_pose = pose;
+  const bool check_map = !(mode_ & MoveAction::Goal::IGNORE_OBSTACLES);
+  for (int i = 0; i < samples; i++) {
+    scl_.step(target_pose_, current_pose, control_duration_, backwards_);
+
+    geometry_msgs::msg::Pose2D p;
+    p.x = current_pose.position.x;
+    p.y = current_pose.position.y;
+    p.theta = tf2::getYaw(current_pose.orientation);
+
+    if (check_map && !local_collision_checker_->isCollisionFree(p, i == 0)) {
+      return true;
+    }
+
+    if (check_rival_collision(p)) {
+      return true;
+    }
+
+    last_ok_pose = current_pose;
+    const double dist_left =
+      distanceToTarget(current_pose, target_pose_.position, target_angle_, backwards_);
+    if (dist_left < 0.005) {
+      return false;
+    }
+  }
+  return false;
+}
+
+ResultStatus MoveCoords::updateVel(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
+  geometry_msgs::msg::Twist & out_vel)
+{
+  double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_);
+
+  if (dist_left <= 0.001) {
+    out_vel.linear.x = 0;
+    out_vel.angular.z = 0;
+    return ResultStatus{Status::SUCCEEDED};
+  }
+
+  const double current_yaw = tf2::getYaw(pose.orientation);
+  double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
+
+  geometry_msgs::msg::Pose last_ok_pose;
+  if (collisionDetection(pose, last_ok_pose)) {
+    dist_left = distanceToTarget(
+      pose, last_ok_pose.position, tf2::getYaw(last_ok_pose.orientation), backwards_);
+    if (dist_left <= 0.02) {
+      out_vel.linear.x = 0;
+      out_vel.angular.z = 0;
+    } else {
+      scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
+      scl_.calculate_vel(last_ok_pose, pose, out_vel, backwards_);
+    }
+
+    last_speed_ = out_vel.linear.x;
+
+    RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
+    RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max);
+
+    return ResultStatus{Status::RUNNING};
+  }
+
+  if (dist_left <= 0.02) {
+    out_vel.linear.x = velocityTarget(dist_left);
+    out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
+    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::MoveCoords, nav2_core::Behavior);

toid_behaviors/src/rotate_acorns.cpp

@@ -0,0 +1,234 @@
+#include "toid_behaviors/rotate_acorns.hpp"
+
+#include <cmath>
+
+#include "angles/angles.h"
+#include "tf2/convert.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+RotateAcorns::RotateAcorns() : SimpleMove<RotateAction>() {}
+RotateAcorns::~RotateAcorns() {}
+
+void RotateAcorns::configureCB()
+{
+  auto node = node_.lock();
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_vel", rclcpp::ParameterValue(2.0));
+
+  node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".min_angular_vel", rclcpp::ParameterValue(0.1));
+  node->get_parameter(behavior_name_ + ".min_angular_vel", min_angular_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_accel", rclcpp::ParameterValue(4.0));
+  node->get_parameter(behavior_name_ + ".max_angular_accel", max_angular_accel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(0.5));
+  node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".lookahead", rclcpp::ParameterValue(0.5));
+  node->get_parameter(behavior_name_ + ".lookahead", lookahead_);
+}
+
+
+void RotateAcorns::activateCB()
+{
+  auto node = node_.lock();
+  using namespace std::placeholders;
+  acorn_pose_sub_ = node->create_subscription<PoseStamped>(
+    "closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&RotateAcorns::acorn_position_cb, this, _1));
+  distance_ = 1.0;
+}
+
+void RotateAcorns::deactivateCB() { 
+    acorn_pose_sub_.reset(); 
+}
+
+void RotateAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
+{
+  geometry_msgs::msg::PoseStamped pose_local;
+  geometry_msgs::msg::PoseStamped pose_global;
+  try {
+    pose_local = tf_->transform(*msg, robot_base_frame_);
+    pose_global = tf_->transform(*msg, global_frame_);
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]...");
+    return;
+  }
+
+  double x = pose_local.pose.position.x;
+  double y = pose_local.pose.position.y;
+  if (x * x + y * y > distance_ + 0.005) {
+    return;
+  }
+
+  double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation));
+
+  if(tf2::getYaw(pose_local.pose.orientation) > 0) {
+    yaw += M_PI;
+  }
+
+  yaw += M_PI/2;
+
+
+  pose_global.pose.position.x += std::cos(yaw) * 0.0 - std::sin(yaw) * - 0.005;
+  pose_global.pose.position.y += std::sin(yaw) * 0.0 + std::cos(yaw) * - 0.005;
+  tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation);
+
+  yaw = angles::normalize_angle(yaw);
+
+  std::lock_guard _lock(mutex_);
+  auto[a,b,c] = avg_.push(pose_global.pose);
+  distance_ = x * x + y * y;
+  new_target_angle_ = c;
+  
+}
+
+ResultStatus RotateAcorns::onStart(
+  const std::shared_ptr<const RotateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+  const geometry_msgs::msg::Twist & vel)
+{
+  std::lock_guard _lock(mutex_);
+  target_angle_ = new_target_angle_;
+  min_turn_angle_ = abs(command->min_angle);
+  initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0;
+  max_angular_speed_ = command->max_speed;
+  mode_ = command->mode;
+  distance_ = 1.0;
+  avg_.reset();
+
+  if (command->max_speed == 0) {
+    auto node = node_.lock();
+    node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
+  }
+
+  geometry_msgs::msg::Pose2D pose2d;
+  pose2d.x = pose.position.x;
+  pose2d.y = pose.position.y;
+  pose2d.theta = initial_direction_;
+  local_collision_checker_->isCollisionFree(pose2d, true);
+
+  last_angle_ = tf2::getYaw(pose.orientation);
+
+  angular_speed_ = vel.angular.z;
+  last_time_ = clock_->now().seconds();
+  return ResultStatus{Status::SUCCEEDED};
+}
+
+void RotateAcorns::calc_err_and_sign(
+  double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign)
+{
+  err = angles::shortest_angular_distance(current_yaw, target_angle_);
+  sign = (err < 0) ? -1.0 : 1.0;
+  err = std::abs(err);
+
+  if (min_turn_angle > 0.0) {
+    const double angle_change = angles::shortest_angular_distance(last_angle, current_yaw);
+    min_turn_angle = std::max(0.0, min_turn_angle - initial_direction_ * angle_change);
+    err = std::max(initial_direction_ * sign * err, 0.0);
+    err = std::max(min_turn_angle, err);
+    sign = initial_direction_;
+  }
+}
+
+double RotateAcorns::calc_speed(
+  const double err, const double sign, const double angular_speed)
+{
+  const double upper_vel_ = angular_speed + max_angular_accel_ * control_duration_;
+  const double lower_vel_ = angular_speed - max_angular_accel_ * control_duration_;
+
+  const double speed_until_overshoot = std::sqrt(2.0 * max_angular_decel_ * std::fabs(err));
+
+  const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
+  const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);
+  return speed;
+}
+
+ResultStatus RotateAcorns::updateVel(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
+  geometry_msgs::msg::Twist & out_vel)
+{
+  {
+    std::lock_guard _lock(mutex_);
+    target_angle_ = new_target_angle_;
+  }
+  const double current_yaw = tf2::getYaw(pose.orientation);
+  double min_turn_angle = min_turn_angle_;
+  double angular_speed = angular_speed_;
+  double err, sign;
+
+  calc_err_and_sign(last_angle_, current_yaw, min_turn_angle, err, sign);
+
+  if (!(mode_ & RotateAction::Goal::IGNORE_OBSTACLES)) {
+    err = check_space(pose, err, sign);
+  }
+
+  double speed = 0.0;
+
+  if (err != 0.0) {
+    speed = calc_speed(err, sign, angular_speed);
+  }
+
+  if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.004) {
+    return ResultStatus{Status::SUCCEEDED};
+  }
+
+  min_turn_angle_ = min_turn_angle;
+  last_angle_ = current_yaw;
+  angular_speed_ = speed;
+  out_vel.angular.z = speed;
+  return ResultStatus{Status::RUNNING};
+}
+
+double RotateAcorns::check_space(
+  const geometry_msgs::msg::Pose pose, const double e, const double sign)
+{
+  geometry_msgs::msg::Pose2D pose2d;
+  pose2d.x = pose.position.x;
+  pose2d.y = pose.position.y;
+  double initial_theta = tf2::getYaw(pose.orientation);
+  pose2d.theta = initial_theta;
+  const double step_size = 0.1;
+  const double err = std::min(e, 1.0);
+  const bool check_map = !(mode_ & RotateAction::Goal::IGNORE_OBSTACLES);
+
+  for (int i = 1; i < err / step_size; i++) {
+    pose2d.theta += sign * step_size;
+
+    if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
+      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+      return step_size * (i - 1);
+    }
+
+    if (check_rival_collision(pose2d)) {
+      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+      return step_size * (i - 1);
+    }
+  }
+
+  pose2d.theta = initial_theta + sign * err;
+
+  if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+    return step_size * ((int)(err / step_size));
+  }
+
+  if (check_rival_collision(pose2d)) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+    return step_size * ((int)(err / step_size));
+  }
+
+  return e;
+}
+
+}  // namespace toid
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::RotateAcorns, nav2_core::Behavior);

toid_behaviors/src/scl.cpp

@@ -0,0 +1,79 @@
+#include "toid_behaviors/scl.hpp"
+
+#include <cmath>
+
+#include "angles/angles.h"
+#include "nav2_util/geometry_utils.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+void SmoothControlLaw::calculate_vel(
+  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
+  geometry_msgs::msg::Twist & out_speed, bool backwards)
+{
+  double r, phi, delta;
+  egocentric_polar(target, current, backwards, r, phi, delta);
+
+  double curvature = this->curvature(r, phi, delta);
+  curvature = backwards ? -curvature : curvature;
+
+  double v = v_linear_max / (1.0 + bbeta * std::pow(fabs(curvature), lam));
+
+  v = std::min(v_linear_max * (r / slowdown_radius), v);
+  v = std::clamp(v, v_linear_min, v_linear_max);
+  v = backwards ? -v : v;
+
+  const double w = curvature * v;
+  const double w_bound = std::clamp(w, -v_angular_max, v_angular_max);
+
+  v = (curvature != 0.0) ? (w_bound / curvature) : v;
+  out_speed.linear.x = v;
+  out_speed.angular.z = w_bound;
+}
+
+double SmoothControlLaw::curvature(double r, double phi, double delta)
+{
+  if (r < 1e-6) {
+    return 0.0;
+  }
+  const float prop = k_delta * (delta - std::atan(-k_phi * phi));
+  const float feedback = (1 + k_phi / (1 + (k_phi * phi) * (k_phi * phi))) * std::sin(delta);
+
+  return -1.0 / r * (prop + feedback);
+}
+
+void SmoothControlLaw::egocentric_polar(
+  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
+  double & r, double & phi, double & delta)
+{
+  const double dx = target.position.x - current.position.x;
+  const double dy = target.position.y - current.position.y;
+  const double los = backwards ? (std::atan2(-dy, dx) + M_PI) : std::atan2(-dy, dx);
+
+  const double ttheta = tf2::getYaw(target.orientation);
+  const double ctheta = tf2::getYaw(current.orientation);
+
+  r = std::hypot(dx, dy);
+  phi = angles::normalize_angle(ttheta + los);
+  delta = angles::normalize_angle(ctheta + los);
+}
+
+void SmoothControlLaw::step(
+  const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt,
+  bool backwards)
+{
+  geometry_msgs::msg::Twist twist;
+  calculate_vel(target, current, twist, backwards);
+
+  double theta = tf2::getYaw(current.orientation);
+  double dx = twist.linear.x * dt * cos(theta);
+  double dy = twist.linear.x * dt * sin(theta);
+  current.orientation =
+    nav2_util::geometry_utils::orientationAroundZAxis(theta + twist.angular.z * dt);
+  current.position.x += dx;
+  current.position.y += dy;
+}
+
+}  // namespace toid

toid_behaviors/src/simple_move.cpp

@@ -0,0 +1 @@
+#include "toid_behaviors/simple_move.hpp"

toid_behaviors/src/simple_rotate.cpp

@@ -0,0 +1,173 @@
+#include "toid_behaviors/simple_rotate.hpp"
+
+#include <cmath>
+
+#include "angles/angles.h"
+#include "tf2/convert.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+SimpleRotateBehavior::SimpleRotateBehavior() : SimpleMove<RotateAction>() {}
+SimpleRotateBehavior::~SimpleRotateBehavior() {}
+
+void SimpleRotateBehavior::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.2));
+  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(4.0));
+  node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".lookahead", rclcpp::ParameterValue(0.5));
+  node->get_parameter(behavior_name_ + ".lookahead", lookahead_);
+}
+
+ResultStatus SimpleRotateBehavior::onStart(
+  const std::shared_ptr<const RotateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+  const geometry_msgs::msg::Twist & vel)
+{
+  target_angle_ = angles::normalize_angle(command->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;
+
+  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 SimpleRotateBehavior::calc_err_and_sign(
+  double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign)
+{
+  err = angles::shortest_angular_distance(current_yaw, target_angle_);
+  sign = (err < 0) ? -1.0 : 1.0;
+  err = std::abs(err);
+
+  if (min_turn_angle > 0.0) {
+    const double angle_change = angles::shortest_angular_distance(last_angle, current_yaw);
+    min_turn_angle = std::max(0.0, min_turn_angle - initial_direction_ * angle_change);
+    err = std::max(initial_direction_ * sign * err, 0.0);
+    err = std::max(min_turn_angle, err);
+    sign = initial_direction_;
+  }
+}
+
+double SimpleRotateBehavior::calc_speed(
+  const double err, const double sign, const double angular_speed)
+{
+  const double upper_vel_ = angular_speed + max_angular_accel_ * control_duration_;
+  const double lower_vel_ = angular_speed - max_angular_accel_ * control_duration_;
+
+  const double speed_until_overshoot = std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
+
+  const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
+  const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);
+  return speed;
+}
+
+ResultStatus SimpleRotateBehavior::updateVel(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
+  geometry_msgs::msg::Twist & out_vel)
+{
+  const double current_yaw = tf2::getYaw(pose.orientation);
+  double min_turn_angle = min_turn_angle_;
+  double angular_speed = angular_speed_;
+  double err, sign;
+
+  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.01) {
+    return ResultStatus{Status::SUCCEEDED};
+  }
+
+  min_turn_angle_ = min_turn_angle;
+  last_angle_ = current_yaw;
+  angular_speed_ = speed;
+  out_vel.angular.z = speed;
+  return ResultStatus{Status::RUNNING};
+}
+
+double SimpleRotateBehavior::check_space(
+  const geometry_msgs::msg::Pose pose, const double e, const double sign)
+{
+  geometry_msgs::msg::Pose2D pose2d;
+  pose2d.x = pose.position.x;
+  pose2d.y = pose.position.y;
+  double initial_theta = tf2::getYaw(pose.orientation);
+  pose2d.theta = initial_theta;
+  const double step_size = 0.1;
+  const double err = std::min(e, 1.0);
+  const bool check_map = !(mode_ & RotateAction::Goal::IGNORE_OBSTACLES);
+
+  for (int i = 1; i < err / step_size; i++) {
+    pose2d.theta += sign * step_size;
+
+    if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
+      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+      return step_size * (i - 1);
+    }
+
+    if (check_rival_collision(pose2d)) {
+      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+      return step_size * (i - 1);
+    }
+  }
+
+  pose2d.theta = initial_theta + sign * err;
+
+  if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+    return step_size * ((int)(err / step_size));
+  }
+
+  if (check_rival_collision(pose2d)) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+    return step_size * ((int)(err / step_size));
+  }
+
+  return e;
+}
+
+}  // namespace toid
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::SimpleRotateBehavior, nav2_core::Behavior);

toid_behaviors/src/simple_translate_x.cpp

@@ -0,0 +1,102 @@
+#include "toid_behaviors/simple_translate_x.hpp"
+
+#include <cmath>
+
+#include "angles/angles.h"
+#include "tf2/convert.hpp"
+
+namespace toid
+{
+
+SimpleTranslateXBehavior::SimpleTranslateXBehavior() : SimpleMove<TranslateAction>() {}
+SimpleTranslateXBehavior::~SimpleTranslateXBehavior() {}
+
+void SimpleTranslateXBehavior::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.007));
+  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_);
+}
+
+ResultStatus SimpleTranslateXBehavior::onStart(
+  const std::shared_ptr<const TranslateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+  const geometry_msgs::msg::Twist & vel)
+{
+  target_distance_ = command->distance;
+  target_angle_ = tf2::getYaw(pose.orientation);
+  target_sign_ = (target_distance_ < 0) ? -1.0 : 1.0;
+  max_vel_speed_ = command->max_speed;
+
+  if(command->max_speed == 0) {
+    auto node = node_.lock();
+    node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
+  }
+
+  target_distance_ *= target_sign_;
+
+  initial_pose_ = pose;
+  speed_ = vel.angular.x;
+
+  return ResultStatus{Status::SUCCEEDED};
+}
+
+ResultStatus SimpleTranslateXBehavior::updateVel(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
+  geometry_msgs::msg::Twist & out_vel)
+{
+  const double current_yaw = tf2::getYaw(pose.orientation);
+  const double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
+
+  const double dx = pose.position.x - initial_pose_.position.x;
+  const double dy = pose.position.y - initial_pose_.position.y;
+  const double dist_passed = target_sign_ * (dx * cos(target_angle_) + dy * sin(target_angle_));
+
+  const double lower_bound = speed_ - control_duration_ * max_vel_accel_;
+  const double upper_bound = speed_ + control_duration_ * max_vel_accel_;
+
+  float vel = std::clamp(max_vel_speed_, lower_bound, upper_bound);
+  float max_vel_to_stop =
+    0.8 * std::sqrt(2.0 * max_vel_decel_ * std::abs(target_distance_ - dist_passed));
+
+  vel = std::min(vel, max_vel_to_stop);
+  const double w = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
+
+  if (dist_passed >= target_distance_) {
+    out_vel.linear.x = 0;
+    out_vel.angular.z = 0;
+    return ResultStatus{Status::SUCCEEDED};
+  }
+
+  out_vel.linear.x = target_sign_ * vel;
+  out_vel.angular.z = w;
+
+  speed_ = vel;
+
+  return ResultStatus{Status::RUNNING};
+}
+
+}  // namespace toid
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::SimpleTranslateXBehavior, nav2_core::Behavior);

toid_behaviors/toid_behaviors.xml

@@ -0,0 +1,19 @@
+<class_libraries>
+    <library path="toid_behaviors">
+        <class type="toid::SimpleRotateBehavior" base_class_type="nav2_core::Behavior">
+            <description></description>
+        </class>
+        <class type="toid::SimpleTranslateXBehavior" base_class_type="nav2_core::Behavior">
+            <description></description>
+        </class>
+        <class type="toid::MoveCoords" base_class_type="nav2_core::Behavior">
+            <description></description>
+        </class>
+        <class type="toid::ApproachAcorns" base_class_type="nav2_core::Behavior">
+            <description></description>
+        </class>
+        <class type="toid::RotateAcorns" base_class_type="nav2_core::Behavior">
+            <description></description>
+        </class>
+    </library>
+</class_libraries>

toid_bot_description/rviz/default.rviz

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

toid_bot_description/src/toid_bot_control.xacro

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

toid_bot_description/src/toid_bot_description.urdf

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

toid_bt/CMakeLists.txt

@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.8)
+project(toid_bt)
+
+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
+  angles
+  behaviortree_cpp
+  behaviortree_ros2
+  btcpp_ros2_interfaces
+  nav2_util
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  geometry_msgs
+  toid_msgs
+  std_msgs
+  std_srvs
+)
+
+foreach(PACKAGE ${PACKAGE_DEPS})
+  find_package(${PACKAGE} REQUIRED)
+endforeach()
+
+set(SOURCES
+    src/bt_executor.cpp
+)
+
+add_executable(toid_bt ${SOURCES})
+
+target_include_directories(
+  toid_bt
+  PRIVATE
+  include
+)
+
+ament_target_dependencies(toid_bt ${PACKAGE_DEPS})
+
+install(TARGETS toid_bt DESTINATION lib/${PROJECT_NAME})
+
+install(
+  DIRECTORY 
+    launch 
+    params
+    behavior_trees
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+target_compile_features(
+  toid_bt PUBLIC
+  c_std_99
+  cxx_std_17
+)
+
+
+ament_package()
+

toid_bt/LICENSE

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

toid_bt/behavior_trees/behavior1.xml

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

toid_bt/behavior_trees/calibration_routines.xml

@@ -0,0 +1,201 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="track_calib">
+    <Sequence>
+      <SetBlackboard value="0.265"
+                     output_key="width"/>
+      <WaitPullPin />
+      <Sleep msec="1000"/>
+      <ZeroOdom service_name=""/>
+      <RotateTowards x="0.4"
+                     y="0.0"
+                     max_speed="0.000000"
+                     min_angle="0.000000"
+                     action_name=""/>
+      <Sequence>
+        <Sleep msec="1000"/>
+        <MovePointSimple x="0.4"
+                         y="0"
+                         theta="0"
+                         max_speed="0.10000"
+                         backwards="false"
+                         action_name=""/>
+        <Sleep msec="500"/>
+        <RotateSimple angle="0"
+                      max_speed="0.500000"
+                      min_angle="700"
+                      action_name=""/>
+      </Sequence>
+      <ForceSuccess>
+        <DetectStuck timeout="1.000000">
+          <MovePointSimple x="-1.0"
+                           y="0"
+                           theta="0"
+                           max_speed="0.070000"
+                           backwards="true"
+                           action_name=""/>
+        </DetectStuck>
+      </ForceSuccess>
+      <SetWidth width="{width}"
+                count="2"
+                new_width="{width}"
+                service_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="wheel_ratio_calibration">
+    <Sequence>
+      <Sleep msec="5000"/>
+      <ZeroOdom service_name=""/>
+      <WaitPullPin />
+      <Sleep msec="1000"/>
+      <Repeat num_cycles="5">
+        <Sequence>
+          <MovePointSimple x="1.1"
+                           y="0"
+                           theta="0"
+                           max_speed="0.30000"
+                           backwards="false"
+                           action_name=""/>
+          <Sleep msec="1000"/>
+          <RotateSimple angle="180"
+                        max_speed="1.300000"
+                        min_angle="10"
+                        action_name=""/>
+          <Sleep msec="500"/>
+          <MovePointSimple x="0.35"
+                           y="0"
+                           theta="180"
+                           max_speed="0.300000"
+                           backwards="false"
+                           action_name=""/>
+          <RotateSimple angle="0"
+                        max_speed="1.300000"
+                        min_angle="-10"
+                        action_name=""/>
+          <Sleep msec="500"/>
+          <MovePointSimple x="1.1"
+                           y="0"
+                           theta="0"
+                           max_speed="0.30000"
+                           backwards="false"
+                           action_name=""/>
+          <Sleep msec="500"/>
+          <RotateSimple angle="180"
+                        max_speed="1.300000"
+                        min_angle="-10"
+                        action_name=""/>
+          <Sleep msec="500"/>
+          <MovePointSimple x="0.35"
+                           y="0"
+                           theta="180"
+                           max_speed="0.300000"
+                           backwards="false"
+                           action_name=""/>
+          <RotateSimple angle="0"
+                        max_speed="1.300000"
+                        min_angle="10"
+                        action_name=""/>
+        </Sequence>
+      </Repeat>
+      <ForceSuccess>
+        <DetectStuck timeout="1.000000">
+          <MovePointSimple x="-0.2"
+                           y="0"
+                           theta="0"
+                           max_speed="0.05"
+                           backwards="true"
+                           action_name=""/>
+        </DetectStuck>
+      </ForceSuccess>
+      <Sleep msec="1000"/>
+      <EndCalib service_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="wheel_size">
+    <Sequence>
+      <WaitPullPin />
+      <Sleep msec="1000"/>
+      <ZeroOdom service_name=""/>
+      <Sleep msec="1000"/>
+      <MovePointSimple x="1.200"
+                       y="0"
+                       theta="0"
+                       max_speed="0.250000"
+                       backwards="false"
+                       action_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Decorator ID="DetectStuck">
+      <input_port name="timeout"
+                  default="1.000000"
+                  type="double"/>
+    </Decorator>
+    <Action ID="EndCalib">
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
+    <Action ID="MovePointSimple">
+      <input_port name="x"
+                  type="double"/>
+      <input_port name="y"
+                  type="double"/>
+      <input_port name="theta"
+                  type="double"/>
+      <input_port name="max_speed"
+                  default="0.000000"
+                  type="double"/>
+      <input_port name="backwards"
+                  default="false"
+                  type="bool"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="RotateSimple">
+      <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="RotateTowards">
+      <input_port name="x"
+                  type="double"/>
+      <input_port name="y"
+                  type="double"/>
+      <input_port name="max_speed"
+                  default="0.000000"
+                  type="double"/>
+      <input_port name="min_angle"
+                  default="0.000000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="SetWidth">
+      <input_port name="width"
+                  type="double"/>
+      <input_port name="count"
+                  default="1"
+                  type="int"/>
+      <output_port name="new_width"
+                   type="double"/>
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
+    <Action ID="ZeroOdom">
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
+  </TreeNodesModel>
+
+</root>

toid_bt/behavior_trees/toid_behaviors.btproj

@@ -0,0 +1,92 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" project_name="Project">
+    <include path="behavior1.xml"/>
+    <include path="calibration_routines.xml"/>
+    <!-- Description of Node Models (used by Groot) -->
+    <TreeNodesModel>
+        <Action ID="ApproachAcorns">
+            <input_port name="x" type="double"/>
+            <input_port name="y" type="double"/>
+            <input_port name="theta" type="double"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="backwards" default="false" type="bool"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Decorator ID="DetectStuck">
+            <input_port name="timeout" default="1.000000" type="double"/>
+        </Decorator>
+        <Action ID="EndCalib">
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+        <Decorator ID="InPose">
+            <input_port name="timeout" default="1.000000" type="double"/>
+        </Decorator>
+        <Action ID="MovePointSimple">
+            <input_port name="x" type="double"/>
+            <input_port name="y" type="double"/>
+            <input_port name="theta" type="double"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="backwards" default="false" type="bool"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="RotateAcorns">
+            <input_port name="angle" type="double"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="min_angle" default="0.000000" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="RotateSimple">
+            <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="RotateTowards">
+            <input_port name="x" type="double"/>
+            <input_port name="y" type="double"/>
+            <input_port name="backwards" default="false" type="bool"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="min_angle" default="0.000000" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="Seq1">
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+        <Action ID="Seq2">
+            <input_port name="text" type="std::string"/>
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+        <Action ID="Seq3">
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+        <Condition ID="SetInitialPose">
+            <input_port name="y" default="0.000000" type="double">Y position in meters</input_port>
+            <input_port name="theta" default="0.000000" type="double">Heading in degrees</input_port>
+            <input_port name="frame_id" default="map" type="std::string">Reference frame</input_port>
+            <input_port name="x" default="0.000000" type="double">X position in meters</input_port>
+            <input_port name="topic_name" default="__default__placeholder__" type="std::string">Topic name</input_port>
+        </Condition>
+        <Action ID="SetParameter">
+            <input_port name="parameter" type="std::string"/>
+            <input_port name="value" type="std::string"/>
+            <input_port name="node" type="std::string"/>
+        </Action>
+        <Action ID="SetWidth">
+            <input_port name="width" type="double"/>
+            <input_port name="count" default="1" type="int"/>
+            <output_port name="new_width" type="double"/>
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+        <Action ID="TranslateX">
+            <input_port name="x" type="double"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="WaitPullPin">
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="ZeroOdom">
+            <input_port name="service_name" type="std::string">Service name</input_port>
+        </Action>
+    </TreeNodesModel>
+</root>

toid_bt/include/toid_bt/bt_executor.hpp

@@ -0,0 +1,47 @@
+#pragma once
+#include "rclcpp/rclcpp.hpp"
+
+#include "behaviortree_ros2/tree_execution_server.hpp"
+#include "behaviortree_cpp/loggers/bt_cout_logger.h"
+
+#include "tf2_ros/buffer.hpp"
+#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 {
+      public:
+        TreeExecutor(const rclcpp::NodeOptions opts);
+        void onTreeCreated(BT::Tree& tree) override;
+        
+        void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override;
+
+        std::optional<std::string> onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override;
+
+        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:
+        std::shared_ptr<BT::StdCoutLogger>          logger_cout_;
+        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

@@ -0,0 +1,9 @@
+#pragma once
+#include <functional>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+namespace toid {
+    using PoseFunc = std::function<void(geometry_msgs::msg::PoseStamped &)>;
+    using FlipFunc = std::function<void(std::string &)>;
+}

toid_bt/include/toid_bt/plugins/empty_srv_action.hpp

@@ -0,0 +1,30 @@
+#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 EmptySrvNode : public BT::RosServiceNode<std_srvs::srv::Empty>
+{
+public:
+  EmptySrvNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosServiceNode<std_srvs::srv::Empty>(name, conf, params)
+  {
+  }
+
+  bool setRequest(typename Request::SharedPtr &) override { return true; }
+
+  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/end_calib_action.hpp

@@ -0,0 +1,59 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+#include "toid_msgs/srv/send_double.hpp"
+
+namespace toid
+{
+
+class EndCalibNode : public BT::RosServiceNode<toid_msgs::srv::SendDouble>
+{
+public:
+  EndCalibNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
+    PoseFunc pose)
+  : BT::RosServiceNode<toid_msgs::srv::SendDouble>(name, conf, params), get_pose(pose)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<double>("width"),
+      BT::InputPort<int>("count", 1, {}),
+      BT::OutputPort<double>("new_width"),
+      //BT::InputPort<double>("options"),
+    });
+  }
+
+  bool setRequest(typename Request::SharedPtr & request) override
+  {
+    auto width = getInput<double>("width").value();
+    double count = getInput<int>("count").value();
+
+    geometry_msgs::msg::PoseStamped pose;
+    get_pose(pose);
+
+    double theta = tf2::getYaw(pose.pose.orientation);
+    double new_width = width * (1 + (theta / (2 * M_PI * count)));
+    request->data = new_width;
+
+    RCLCPP_INFO(logger(), "width is %lf", new_width);
+
+    setOutput("new_width", new_width);
+    return true;
+  }
+
+  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  PoseFunc get_pose;
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/in_position_decorator.hpp

@@ -0,0 +1,74 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+
+namespace toid
+{
+
+class InPositionNode : public BT::DecoratorNode
+{
+public:
+  InPositionNode(
+    const std::string & name, const BT::NodeConfig & conf, PoseFunc get_pose)
+  : BT::DecoratorNode(name, conf), get_pose(get_pose)
+  {
+  }
+
+  static BT::PortsList providedPorts() {
+    return {
+        BT::InputPort<double>("timeout", 1, {})
+    };
+  }
+
+  private:
+    bool checkIfPosesAreClose(geometry_msgs::msg::PoseStamped &poseA, geometry_msgs::msg::PoseStamped &poseB) {
+        const double dx = abs(poseA.pose.position.x - poseB.pose.position.x);
+        return dx < 0.005;
+    }
+
+    bool checkIfInPose(geometry_msgs::msg::PoseStamped &pose) {
+        const double dx = abs(0.2575 - pose.pose.position.x);
+        return dx < 0.005;
+    }
+
+    BT::NodeStatus tick() override
+    {
+        if(status() == BT::NodeStatus::IDLE) {
+            setStatus(BT::NodeStatus::RUNNING);
+            last_pos_change = clock.now();
+        }
+
+        double timeout = getInput<double>("timeout").value_or(1.0);
+
+
+        geometry_msgs::msg::PoseStamped pose;
+        get_pose(pose);
+
+        if(checkIfInPose(pose)) {
+            if(checkIfPosesAreClose(last_pos, pose)) {
+                if(rclcpp::Time(pose.header.stamp) - last_pos_change > rclcpp::Duration::from_seconds(timeout)) {
+                    haltChild();
+                    return BT::NodeStatus::SUCCESS;
+                }
+            } else {
+                last_pos = pose;
+                last_pos_change = pose.header.stamp;
+            }
+        }
+
+        const BT::NodeStatus child_status = child_node_->executeTick();
+        return child_status;
+    }
+
+  geometry_msgs::msg::PoseStamped last_pos;
+  PoseFunc get_pose;
+  rclcpp::Time last_pos_change;
+  rclcpp::Clock clock;
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/move_coords_action.hpp

@@ -0,0 +1,77 @@
+#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/simple_move_coords.hpp"
+
+namespace toid
+{
+
+class MovePointNode : public BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>
+{
+public:
+  MovePointNode(
+    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)
+  {
+    std::cout << "wtf? " << params.default_port_value << std::endl;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<double>("x"),
+      BT::InputPort<double>("y"),
+      BT::InputPort<double>("theta"),
+      BT::InputPort<double>("max_speed", 0, {}),
+      BT::InputPort<bool>("backwards", false, {}),
+      //BT::InputPort<double>("options"),
+    });
+  }
+
+  bool setGoal(Goal & goal) override
+  {
+    auto x_goal = getInput<double>("x");
+    auto y_goal = getInput<double>("y");
+    auto theta = getInput<double>("theta");
+    auto max_speed = getInput<double>("max_speed").value();
+    auto backwards = getInput<bool>("backwards").value();
+
+    goal.x = x_goal.value();
+    goal.y = y_goal.value();
+    if (!theta.has_value()) {
+      geometry_msgs::msg::PoseStamped pose;
+      get_pose(pose);
+      goal.theta = tf2::getYaw(pose.pose.orientation);
+    } else {
+      goal.theta = angles::from_degrees(theta.value_or(0));
+    }
+
+    goal.max_speed = max_speed;
+    goal.backwards = backwards;
+    goal.mode = 1;
+
+    return true;
+  }
+
+  void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
+
+  BT::NodeStatus onResultReceived(const WrappedResult & wr) override
+  {
+    return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+  }
+
+  BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
+  {
+    RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+    return BT::NodeStatus::FAILURE;
+  }
+
+  PoseFunc get_pose;
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/pull_pin_action.hpp

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

toid_bt/include/toid_bt/plugins/rotate_action.hpp

@@ -0,0 +1,63 @@
+#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/simple_rotate.hpp"
+
+namespace toid
+{
+
+class RotateNode : public BT::RosActionNode<toid_msgs::action::SimpleRotate>
+{
+public:
+  RotateNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
+    PoseFunc pose)
+  : BT::RosActionNode<toid_msgs::action::SimpleRotate>(name, conf, params), get_pose(pose)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<double>("angle", {}), 
+      BT::InputPort<double>("min_angle", 0, {}),
+      BT::InputPort<double>("max_speed", 0, {}),
+      //BT::InputPort<double>("options"),
+    });
+  }
+
+  bool setGoal(Goal & goal) override
+  {
+    auto angle = getInput<double>("angle");
+    auto min_angle = getInput<double>("min_angle");
+    auto max_speed = getInput<double>("max_speed");
+    goal.angle = angle.value();
+    goal.min_angle = min_angle.value();
+    goal.max_speed = max_speed.value();
+    goal.angle = angles::from_degrees(goal.angle);
+    goal.min_angle = angles::from_degrees(goal.min_angle);
+    goal.mode = 1;
+    return true;
+  }
+
+  void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
+
+  BT::NodeStatus onResultReceived(const WrappedResult & wr) override
+  {
+    return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+  }
+
+  BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
+  {
+    RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+    return BT::NodeStatus::FAILURE;
+  }
+
+  PoseFunc get_pose;
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp

@@ -0,0 +1,75 @@
+#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/simple_rotate.hpp"
+
+namespace toid
+{
+
+class RotateTowardsNode : public BT::RosActionNode<toid_msgs::action::SimpleRotate>
+{
+public:
+  RotateTowardsNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
+    PoseFunc pose)
+  : BT::RosActionNode<toid_msgs::action::SimpleRotate>(name, conf, params), get_pose(pose)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    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"),
+    });
+  }
+
+  bool setGoal(Goal & goal) override
+  {
+    auto x = getInput<double>("x").value();
+    auto y = getInput<double>("y").value();
+    auto min_angle = getInput<double>("min_angle").value();
+    auto max_speed = getInput<double>("max_speed").value();
+    auto backwards = getInput<bool>("backwards").value();
+
+    geometry_msgs::msg::PoseStamped pose;
+    get_pose(pose);
+
+    if(!backwards) {
+      goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x);
+    }
+    if(backwards) {
+      goal.angle = std::atan2(pose.pose.position.y - y, pose.pose.position.x - x);
+    }
+    goal.min_angle = angles::from_degrees(min_angle);
+    goal.max_speed = max_speed;
+    goal.mode = 1;
+
+    return true;
+  }
+
+  void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
+
+  BT::NodeStatus onResultReceived(const WrappedResult & wr) override
+  {
+    return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+  }
+
+  BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
+  {
+    RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+    return BT::NodeStatus::FAILURE;
+  }
+
+  PoseFunc get_pose;
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/send_text_action.hpp

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

toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp

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

toid_bt/include/toid_bt/plugins/set_parameter_action.hpp

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

toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp

@@ -0,0 +1,69 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+
+namespace toid
+{
+
+class StuckDetectorNode : public BT::DecoratorNode
+{
+public:
+  StuckDetectorNode(
+    const std::string & name, const BT::NodeConfig & conf, PoseFunc get_pose)
+  : BT::DecoratorNode(name, conf), get_pose(get_pose)
+  {
+  }
+
+  static BT::PortsList providedPorts() {
+    return {
+        BT::InputPort<double>("timeout", 1, {})
+    };
+  }
+
+  private:
+    bool checkIfPosesAreClose(geometry_msgs::msg::PoseStamped &poseA, geometry_msgs::msg::PoseStamped &poseB) {
+        const double dx = poseA.pose.position.x - poseB.pose.position.x;
+        const double dy = poseA.pose.position.y - poseB.pose.position.y;
+        const double dth = abs(tf2::getYaw(poseA.pose.orientation) - tf2::getYaw(poseB.pose.orientation));
+        return dx*dx + dy*dy < 0.004*0.004 && dth < 0.05;
+    }
+
+    BT::NodeStatus tick() override
+    {
+        if(status() == BT::NodeStatus::IDLE) {
+            setStatus(BT::NodeStatus::RUNNING);
+            last_pos_change = clock.now();
+        }
+
+        double timeout = getInput<double>("timeout").value_or(1.0);
+
+
+        geometry_msgs::msg::PoseStamped pose;
+        get_pose(pose);
+
+        if(checkIfPosesAreClose(last_pos, pose)) {
+            if(clock.now() - last_pos_change > rclcpp::Duration::from_seconds(timeout)) {
+                haltChild();
+                return BT::NodeStatus::FAILURE;
+            }
+        } else {
+            last_pos = pose;
+            last_pos_change = clock.now();
+        }
+
+        const BT::NodeStatus child_status = child_node_->executeTick();
+        return child_status;
+    }
+
+  geometry_msgs::msg::PoseStamped last_pos;
+  PoseFunc get_pose;
+  rclcpp::Time last_pos_change;
+  rclcpp::Clock clock;
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/translate_x_action.hpp

@@ -0,0 +1,68 @@
+#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/simple_move_coords.hpp"
+
+namespace toid
+{
+
+class TranslateXNode : public BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>
+{
+public:
+  TranslateXNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
+    PoseFunc pose)
+  : BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>(name, conf, params), get_pose(pose)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<double>("x"),
+      BT::InputPort<double>("max_speed", 0, {}),
+      //BT::InputPort<double>("options"),
+    });
+  }
+
+  bool setGoal(Goal & goal) override
+  {
+    auto x_goal = getInput<double>("x").value();
+    auto max_speed = getInput<double>("max_speed").value();
+
+    geometry_msgs::msg::PoseStamped pose;
+    get_pose(pose);
+
+    double yaw = tf2::getYaw(pose.pose.orientation);
+
+    goal.x = pose.pose.position.x + cos(yaw) * x_goal;
+    goal.y = pose.pose.position.y + sin(yaw) * x_goal;
+    goal.theta = yaw;
+    goal.backwards = x_goal < 0;
+    goal.mode = 1;
+    goal.max_speed = max_speed;
+
+    return true;
+  }
+
+  void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
+
+  BT::NodeStatus onResultReceived(const WrappedResult & wr) override
+  {
+    return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+  }
+
+  BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
+  {
+    RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+    return BT::NodeStatus::FAILURE;
+  }
+
+  PoseFunc get_pose;
+};
+
+}  // namespace toid

toid_bt/launch/bt.launch.py

@@ -0,0 +1,41 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+import os
+
+def generate_launch_description():
+
+    pkg_share = FindPackageShare("").find('toid_bt')
+    bt_params = os.path.join(pkg_share, 'params', 'bt_params.yaml')
+
+    visualize = LaunchConfiguration("visualize")
+    visualize_arg = DeclareLaunchArgument(
+        '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" 
+    )
+
+    bt_executor = Node(
+        package='toid_bt',
+        executable='toid_bt',
+        output='screen',
+        emulate_tty=True,
+        parameters=[bt_params],
+    )
+
+    return LaunchDescription([
+        visualize_arg,
+        use_mock_arg,
+        bt_executor,
+    ])
+

toid_bt/package.xml

@@ -0,0 +1,31 @@
+<?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_bt</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>angles</depend>
+  <depend>behaviortree_ros2</depend>
+  <depend>btcpp_ros2_interfaces</depend>
+  <depend>nav2_util</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>toid_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

toid_bt/params/bt_params.yaml

@@ -0,0 +1,9 @@
+toid_bt:
+  ros__parameters:
+    action_name: "bt_run" 
+    tick_frequency: 100
+    groot2_port: 8420
+    ros_plugins_timeout: 1000
+
+    behavior_trees:
+      - toid_bt/behavior_trees

toid_bt/src/bt_executor.cpp

@@ -0,0 +1,169 @@
+#include "toid_bt/bt_executor.hpp"
+
+#include "behaviortree_cpp/xml_parsing.h"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+#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
+{
+TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts)
+: BT::TreeExecutionServer(std::make_shared<rclcpp::Node>("toid_bt", opts))
+{
+  /* 
+  executeRegistration();
+  std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
+  */
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node(), "base_frame", rclcpp::ParameterValue("base_footprint"));
+  node()->get_parameter("base_frame", base_frame_);
+
+  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)
+{
+  logger_cout_ = std::make_shared<BT::StdCoutLogger>(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);
+
+  factory.registerNodeType<toid::EndCalibNode>(
+    "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose);
+
+  factory.registerNodeType<toid::PullPinNode>("WaitPullPin", BT::RosNodeParams(nh, "/start_plug"));
+
+  factory.registerNodeType<toid::SetParameterNode>(
+    "SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters"));
+
+  factory.registerNodeType<toid::SetInitialPoseNode>(
+    "SetInitialPose", BT::RosNodeParams(nh, "/initialpose"));
+
+  factory.registerNodeType<toid::EmptySrvNode>("ZeroOdom", BT::RosNodeParams(nh, "/zero"));
+
+  factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
+
+  BT::RosNodeParams service_params1(nh, "/sequence1");
+  service_params1.server_timeout = std::chrono::seconds(15);
+  factory.registerNodeType<toid::EmptySrvNode>("Seq1", service_params1);
+
+  BT::RosNodeParams service_params2(nh, "/sequence2");
+  service_params2.server_timeout = std::chrono::seconds(20);
+  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params2, get_to_flip);
+
+  BT::RosNodeParams service_params3(nh, "/sequence3");
+  service_params3.server_timeout = std::chrono::seconds(15);
+  factory.registerNodeType<toid::EmptySrvNode>("Seq3", service_params3);
+
+  factory.registerNodeType<toid::StuckDetectorNode>("DetectStuck", get_pose);
+
+  factory.registerNodeType<toid::InPositionNode>("InPose", get_acorn_pose);
+
+  std::cout << describeCustomNodes() << std::endl;
+}
+
+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)
+{
+  (void)status;
+  logger_cout_.reset();
+
+  if (was_cancelled) {
+    std::cout << "Behavior tree was cancled" << std::endl;
+  }
+
+  return std::nullopt;
+}
+
+std::string TreeExecutor::describeCustomNodes() { return BT::writeTreeNodesModelXML(factory()); }
+
+}  // namespace toid
+
+int main(const int argc, const char * const * argv)
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::NodeOptions options;
+  auto bt_exec_server = std::make_shared<toid::TreeExecutor>(options);
+
+  rclcpp::executors::MultiThreadedExecutor executor(
+    rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
+
+  executor.add_node(bt_exec_server->node());
+  executor.spin();
+  executor.remove_node(bt_exec_server->node());
+
+  rclcpp::shutdown();
+  return 0;
+}

toid_control/launch/toid.launch.py

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

toid_control/src/toid_control.cpp

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

toid_costmaps/CMakeLists.txt

@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.8)
+project(toid_costmaps)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(library_name toid_costmaps)
+
+set(
+  PACKAGE_DEPS
+  rclcpp
+  ament_index_cpp
+  Boost
+  geometry_msgs
+  pluginlib
+  nav_msgs
+  nav2_core
+  nav2_costmap_2d
+  nav2_util
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+  Eigen3
+  toid_msgs
+)
+
+set(
+  SOURCES
+  src/game_elements_layer.cpp
+  src/rival_layer.cpp
+)
+
+find_package(ament_cmake REQUIRED)
+foreach(PACKAGE ${PACKAGE_DEPS})
+  find_package(${PACKAGE} REQUIRED)
+endforeach()
+
+find_package(Boost REQUIRED COMPONENTS json)
+
+
+add_library(${library_name} SHARED ${SOURCES})
+
+target_link_libraries(
+  ${library_name}
+  Boost::json
+)
+
+target_include_directories(
+  ${library_name} 
+  PRIVATE 
+
+  include
+)
+
+ament_target_dependencies(
+  ${library_name}
+  ${PACKAGE_DEPS}
+)
+
+install(TARGETS ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin 
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/
+)
+
+install(
+  DIRECTORY elements
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${PACKAGE_DEPS})
+
+pluginlib_export_plugin_description_file(nav2_costmap_2d toid_costmaps.xml)
+
+ament_package()

toid_costmaps/LICENSE

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

toid_costmaps/elements/elements.json

@@ -0,0 +1,46 @@
+{
+  "game_elements": [
+    {
+      "x": 0.0,
+      "y": 0.7,
+      "width": 0.2,
+      "height": 0.2
+    },
+    {
+      "x": 0.05,
+      "y": 0.3,
+      "width": 0.2,
+      "height": 0.2
+    },
+    {
+      "x": 0.6,
+      "y": 0.0,
+      "width": 0.2,
+      "height": 0.2
+    },
+    {
+      "x": 0.05,
+      "y": 1.1,
+      "width": 0.2,
+      "height": 0.2
+    }
+  ],
+
+  "blue": [
+    {
+      "x": 0.00,
+      "y": 1.55,
+      "width": 0.6,
+      "height": 0.45
+    }
+  ],
+
+  "yellow": [
+    {
+      "x": 2.40,
+      "y": 1.55,
+      "width": 0.6,
+      "height": 0.45
+    }
+  ]
+}

toid_costmaps/include/toid_costmaps/element_info.hpp

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "boost/json.hpp"
+
+namespace toid
+{
+class GameElement
+{
+public:
+  GameElement(double x, double y, double width, double height)
+  : x_(x), y_(y), width_(width), height_(height)
+  {
+  }
+
+  void start(double & x, double & y) const
+  {
+    x = this->x_;
+    y = this->y_;
+  }
+
+  void end(double & x, double & y) const
+  {
+    x = this->x_ + this->width_;
+    y = this->y_ + this->height_;
+  }
+
+private:
+  double x_;
+  double y_;
+  double width_;
+  double height_;
+};
+
+inline GameElement tag_invoke(
+  boost::json::value_to_tag<GameElement>, boost::json::value const & jv)
+{
+    auto const& obj = jv.as_object();
+    return GameElement{
+        boost::json::value_to<double>(obj.at("x")) - 1.5,
+        boost::json::value_to<double>(obj.at("y")) - 1.0,
+        boost::json::value_to<double>(obj.at("width")),
+        boost::json::value_to<double>(obj.at("height"))
+    };
+}
+
+}  // namespace toid

toid_costmaps/include/toid_costmaps/game_elements_layer.hpp

@@ -0,0 +1,58 @@
+#pragma once
+
+#include "jsoncpp/json/json.h"
+#include "nav2_costmap_2d/costmap_layer.hpp"
+#include "nav2_costmap_2d/layer.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "toid_costmaps/element_info.hpp"
+#include "toid_msgs/msg/active_elements.hpp"
+
+namespace toid
+{
+
+class GameElementLayer : public nav2_costmap_2d::CostmapLayer
+{
+  using ActiveElements = toid_msgs::msg::ActiveElements;
+
+public:
+  GameElementLayer() { costmap_ = NULL; }
+  ~GameElementLayer() {}
+
+  void onInitialize() override;
+  void activate() override;
+  void deactivate() override;
+
+  void updateBounds(
+    double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y,
+    double * max_x, double * max_y) override;
+
+  void updateCosts(
+    nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) override;
+
+  void reset() override { return; }
+
+  void onFootprintChanged() override {}
+
+  void placeElement(nav2_costmap_2d::Costmap2D & grid, const GameElement & element);
+
+  bool isClearable() override { return true; }
+
+  void initGameElements();
+
+  void active_elements_cb(ActiveElements::SharedPtr msg);
+
+private:
+  double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
+
+  bool need_recalculation_;
+
+  std::vector<GameElement> game_elements_;
+  std::vector<GameElement> static_elements_;
+  std::vector<std::string> extra_elements_;
+  ActiveElements::SharedPtr active_elements_;
+
+  rclcpp::Subscription<ActiveElements>::SharedPtr active_elements_sub_;
+};
+
+}  // namespace toid

toid_costmaps/include/toid_costmaps/rival_layer.hpp

@@ -0,0 +1,55 @@
+#pragma once
+
+#include "nav2_costmap_2d/costmap_layer.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "toid_msgs/msg/rival.hpp"
+
+
+namespace toid
+{
+
+class RivalLayer : public nav2_costmap_2d::CostmapLayer
+{
+  using Rivals = toid_msgs::msg::Rival;
+
+public:
+  RivalLayer() { costmap_ = NULL; }
+  ~RivalLayer() {}
+
+  void onInitialize() override;
+  void activate() override;
+  void deactivate() override;
+
+  void updateBounds(
+    double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y,
+    double * max_x, double * max_y) override;
+
+  void updateCosts(
+    nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) override;
+
+  void reset() override { return; }
+
+  void onFootprintChanged() override {}
+
+  void placeRival(nav2_costmap_2d::Costmap2D & grid, double x, double y);
+
+  bool isClearable() override { return true; }
+
+  void rival_cb(Rivals::SharedPtr msg);
+
+private:
+  double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
+
+  bool need_recalculation_;
+
+  uint debounce_ = 0;
+
+  Rivals::SharedPtr rivals_;
+
+  rclcpp::Subscription<Rivals>::SharedPtr rival_sub_;
+
+  double rival_size_ = 0.15;
+};
+
+}  // namespace toid

toid_costmaps/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>toid_costmaps</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="82343504+pimpest@users.noreply.github.com">Pimpest</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>angles</depend>
+  <depend>ament_index_cpp</depend>
+  <depend>boost</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_core</depend>
+  <depend>pluginlib</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>toid_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

toid_costmaps/src/game_elements_layer.cpp

@@ -0,0 +1,138 @@
+#include "toid_costmaps/game_elements_layer.hpp"
+
+#include <fstream>
+
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "nav2_util/node_utils.hpp"
+
+namespace toid
+{
+
+void GameElementLayer::onInitialize()
+{
+  auto node = node_.lock();
+  if (!node) {
+    return;
+  }
+}
+
+void GameElementLayer::activate()
+{
+  auto node = node_.lock();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name_ + ".extra_elements", rclcpp::ParameterValue(std::vector<std::string>{}));
+  node->get_parameter(name_ + ".extra_elements", extra_elements_);
+
+  initGameElements();
+
+  active_elements_ = std::make_unique<ActiveElements>();
+  active_elements_->active = std::vector<bool>(game_elements_.size(), true);
+
+  using namespace std::placeholders;
+  active_elements_sub_ = node->create_subscription<ActiveElements>(
+    "/active_elements", 1, std::bind(&GameElementLayer::active_elements_cb, this, _1));
+}
+
+void GameElementLayer::deactivate()
+{
+  auto node = node_.lock();
+  active_elements_sub_.reset();
+  active_elements_.reset();
+  game_elements_.clear();
+  static_elements_.clear();
+  extra_elements_.clear();
+}
+
+void GameElementLayer::active_elements_cb(ActiveElements::SharedPtr msg)
+{
+  active_elements_ = msg;
+}
+
+void GameElementLayer::updateBounds(
+  double, double, double, double * min_x, double * min_y, double * max_x, double * max_y)
+{
+  touch(1.50, 1.0, min_x, min_y, max_x, max_y);
+  touch(-1.50, -1.0, min_x, min_y, max_x, max_y);
+}
+
+void GameElementLayer::updateCosts(nav2_costmap_2d::Costmap2D & grid, int, int, int, int)
+{
+  ulong idx = 0;
+  for (const auto & elem : game_elements_) {
+    if (active_elements_->active.size() > idx && active_elements_->active.at(idx)) {
+      placeElement(grid, elem);
+    }
+    idx++;
+  }
+
+  for (const auto & elem : static_elements_) {
+    placeElement(grid, elem);
+  }
+}
+
+void GameElementLayer::placeElement(nav2_costmap_2d::Costmap2D & grid, const GameElement & element)
+{
+  bool in_bounds = true;
+  double ex, ey;
+  element.start(ex, ey);
+  uint mx, my;
+  in_bounds &= worldToMap(ex + 0.001, ey + 0.001, mx, my);
+  element.end(ex, ey);
+  uint mmx, mmy;
+  in_bounds &= worldToMap(ex + 0.001, ey + 0.001, mmx, mmy);
+
+  if (in_bounds) {
+    for (uint j = my; j < mmy; j++) {
+      for (uint i = mx; i < mmx; i++) {
+        grid.setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE);
+      }
+    }
+  }
+}
+
+void GameElementLayer::initGameElements()
+{
+  std::string file_path = "/elements/elements.json";
+  std::string base_path = ament_index_cpp::get_package_share_directory("toid_costmaps");
+
+  RCLCPP_INFO(rclcpp::get_logger("toid_costmap"), "Bro hear me out");
+
+  std::ifstream fs(base_path + file_path);
+  if (!fs.is_open()) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("toid_costmap"), "Failed to open elements file: %s%s", base_path.c_str(),
+      file_path.c_str());
+    throw std::runtime_error("File not found");
+  }
+
+  boost::json::stream_parser p;
+  boost::json::error_code ec;
+
+  char buffer[4096];
+  while (fs.read(buffer, sizeof(buffer)) || fs.gcount() > 0) {
+    p.write(buffer, fs.gcount(), ec);
+    if (ec) {
+      RCLCPP_FATAL(rclcpp::get_logger("toid_costmap"), "JsonError: %s", ec.message().c_str());
+      throw std::runtime_error("JsonError");
+    }
+  }
+  p.finish(ec);
+  if (ec) {
+    RCLCPP_FATAL(rclcpp::get_logger("toid_costmap"), "JsonError: %s", ec.message().c_str());
+    throw std::runtime_error("JsonError");
+  }
+
+  boost::json::value jv = p.release();
+
+  game_elements_ = boost::json::value_to<std::vector<GameElement>>(jv.at("game_elements"));
+  for (const auto & t : extra_elements_) {
+    std::vector<GameElement> elements = boost::json::value_to<std::vector<GameElement>>(jv.at(t));
+    static_elements_.insert(static_elements_.end(), elements.begin(), elements.end());
+  }
+}
+
+}  // namespace toid
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::GameElementLayer, nav2_costmap_2d::Layer);

toid_costmaps/src/rival_layer.cpp

@@ -0,0 +1,108 @@
+
+#include "toid_costmaps/rival_layer.hpp"
+
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+void RivalLayer::onInitialize()
+{
+  auto node = node_.lock();
+  if (!node) {
+    return;
+  }
+}
+
+void RivalLayer::activate()
+{
+  auto node = node_.lock();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name_ + ".rival_size", rclcpp::ParameterValue(0.15));
+  node->get_parameter(name_ + ".rival_size", rival_size_);
+
+  using namespace std::placeholders;
+  rival_sub_ = node->create_subscription<Rivals>(
+    "/dynamicObstacle", 1, std::bind(&RivalLayer::rival_cb, this, _1));
+}
+
+void RivalLayer::deactivate()
+{
+  auto node = node_.lock();
+  rival_sub_.reset();
+  rivals_.reset();
+}
+
+void RivalLayer::rival_cb(Rivals::SharedPtr msg)
+{
+  if (msg->point.size() != 0 || debounce_++ > 10) {
+    rivals_ = msg;
+    debounce_ = 0;
+  }
+}
+
+void RivalLayer::updateBounds(
+  double, double, double, double * min_x, double * min_y, double * max_x, double * max_y)
+{
+  touch(1.50, 1.0, min_x, min_y, max_x, max_y);
+  touch(-1.50, -1.0, min_x, min_y, max_x, max_y);
+}
+
+void RivalLayer::updateCosts(nav2_costmap_2d::Costmap2D & grid, int, int, int, int)
+{
+  geometry_msgs::msg::TransformStamped tf_msg;
+  if(!rivals_) {
+    return;
+  }
+
+  try {
+    tf_msg = tf_->lookupTransform(
+      layered_costmap_->getGlobalFrameID(), rivals_->header.frame_id, rivals_->header.stamp);
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 1000, "Failed to transform rival message to costmap");
+    return;
+  }
+
+  for (const auto & rival : rivals_->point) {
+    geometry_msgs::msg::Point point;
+    tf2::doTransform(rival, point, tf_msg);
+    placeRival(grid, point.x, point.y);
+  }
+}
+
+void RivalLayer::placeRival(nav2_costmap_2d::Costmap2D & grid, const double x, const double y)
+{
+  unsigned int mx, my;
+  if (!grid.worldToMap(x, y, mx, my)) {
+    return;  // Center is outside the map bounds
+  }
+
+  double res = grid.getResolution();
+  int cell_radius = static_cast<int>(rival_size_ / res);
+
+  int min_i = std::max(0, static_cast<int>(mx) - cell_radius);
+  int max_i =
+    std::min(static_cast<int>(grid.getSizeInCellsX()) - 1, static_cast<int>(mx) + cell_radius);
+  int min_j = std::max(0, static_cast<int>(my) - cell_radius);
+  int max_j =
+    std::min(static_cast<int>(grid.getSizeInCellsY()) - 1, static_cast<int>(my) + cell_radius);
+
+  for (int i = min_i; i <= max_i; ++i) {
+    for (int j = min_j; j <= max_j; ++j) {
+      double di = static_cast<double>(i) - static_cast<double>(mx);
+      double dj = static_cast<double>(j) - static_cast<double>(my);
+      double distance_sq = di * di + dj * dj;
+      if (distance_sq <= static_cast<double>(cell_radius * cell_radius)) {
+        grid.setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE);
+      }
+    }
+  }
+}
+
+}  // namespace toid
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::RivalLayer, nav2_costmap_2d::Layer);

toid_costmaps/toid_costmaps.xml

@@ -0,0 +1,10 @@
+<class_libraries>
+    <library path="toid_costmaps">
+        <class type="toid::GameElementLayer" base_class_type="nav2_costmap_2d::Layer">
+            <description></description>
+        </class>
+        <class type="toid::RivalLayer" base_class_type="nav2_costmap_2d::Layer">
+            <description></description>
+        </class>
+    </library>
+</class_libraries>

toid_interaction/package.xml

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

toid_interaction/setup.cfg

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

toid_interaction/setup.py

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

toid_interaction/test/test_copyright.py

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

toid_interaction/test/test_flake8.py

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

toid_interaction/test/test_pep257.py

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

toid_interaction/toid_interaction/interaction_node.py

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

toid_interaction/toid_interaction/mechanism/STservo_sdk/__init__.py

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

toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_read.py

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

toid_interaction/toid_interaction/mechanism/STservo_sdk/group_sync_write.py

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

toid_interaction/toid_interaction/mechanism/STservo_sdk/port_handler.py

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

toid_interaction/toid_interaction/mechanism/STservo_sdk/protocol_packet_handler.py

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

toid_interaction/toid_interaction/mechanism/STservo_sdk/scscl.py

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

toid_interaction/toid_interaction/mechanism/STservo_sdk/setup.py

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

toid_interaction/toid_interaction/mechanism/STservo_sdk/sts.py

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

toid_interaction/toid_interaction/mechanism/STservo_sdk/stservo_def.py

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

toid_interaction/toid_interaction/mechanism/motori.py

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

toid_interaction/toid_interaction/mechanism/sekvenca_2026.py

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

toid_interaction/toid_interaction/mechanism/zidovi.py

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

toid_interaction/toid_interaction/mechanism/zidovi_load.py

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

toid_interaction/toid_interaction/mechanism/zupcanik.py

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

toid_lidar/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.8)
+project(toid_lidar)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+# set aditional dependencies
+set(PACKAGE_DEPS
+  rclcpp
+  Eigen3
+  nav2_util
+  sensor_msgs
+  tf2
+  tf2_ros
+  tf2_eigen
+  tf2_geometry_msgs
+  toid_msgs
+  visualization_msgs
+)
+
+foreach(PACKAGE ${PACKAGE_DEPS})
+  find_package(${PACKAGE} REQUIRED)
+endforeach()
+
+set(SOURCES
+    src/toid_lidar.cpp
+    src/toid_lidar_node.cpp
+)
+
+add_executable(toid_lidar ${SOURCES})
+
+target_include_directories(
+  toid_lidar
+  PRIVATE
+  include
+)
+
+install(
+  DIRECTORY 
+    launch 
+    params
+    rviz
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_target_dependencies(toid_lidar ${PACKAGE_DEPS})
+
+install(TARGETS toid_lidar DESTINATION lib/${PROJECT_NAME})
+
+target_compile_features(
+  toid_lidar PUBLIC
+  c_std_99
+  cxx_std_17
+)
+
+ament_package()

toid_lidar/LICENSE

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

toid_lidar/include/toid_lidar/toid_lidar_node.hpp

@@ -0,0 +1,47 @@
+#pragma once
+
+#include "Eigen/Geometry"
+#include "geometry_msgs/msg/point_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+#include "toid_msgs/msg/rival.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+#include "vector"
+
+namespace toid
+{
+
+class ToidRivalDetect : public rclcpp::Node
+{
+  using LaserScan = sensor_msgs::msg::LaserScan;
+  using Rival = toid_msgs::msg::Rival;
+  using PointStamped = geometry_msgs::msg::PointStamped;
+  using Marker = visualization_msgs::msg::Marker;
+
+public:
+  ToidRivalDetect();
+private:
+
+  void process_scan(LaserScan::ConstSharedPtr msg);
+
+  rclcpp::Subscription<LaserScan>::SharedPtr scan_sub_;
+  rclcpp::Publisher<Rival>::SharedPtr rival_pub_;
+  rclcpp::Publisher<Marker>::SharedPtr marker_pub_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buf_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  std::string global_frame_ = "map";
+  double map_width_ = 3.0;
+  double map_height_ = 2.0;
+  bool visualize_ = false;
+  bool closest_ = false;
+
+  rclcpp::Logger logger_ = get_logger();
+  rclcpp::Clock::SharedPtr clock_;
+
+};
+
+}  // namespace toid

toid_lidar/launch/launch.py

@@ -0,0 +1,67 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from pathlib import Path
+
+
+def generate_launch_description():
+
+    basedir = FindPackageShare("").find("toid_lidar")
+
+    lidar_config = PathJoinSubstitution([basedir, "params/lidar.yaml"])
+    rviz_config = PathJoinSubstitution([basedir, "rviz/conf.rviz"])
+
+    visualize = LaunchConfiguration("visualize")
+    draw_markers = LaunchConfiguration("draw_markers")
+    use_closest = LaunchConfiguration("use_closest")
+    lidar_frame = LaunchConfiguration("lidar_frame")
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "visualize",
+                default_value="False",
+                description="Whether to launch rviz2",
+            ),
+            DeclareLaunchArgument(
+                "draw_markers", default_value="False", description="Draw markers"
+            ),
+            DeclareLaunchArgument(
+                "use_closest",
+                default_value="True",
+                description="Use closest point for calibration",
+            ),
+            DeclareLaunchArgument(
+                "lidar_frame",
+                default_value="lidar_frame",
+                description="TF frame of the lidar",
+            ),
+            Node(
+                package="toid_lidar",
+                executable="toid_lidar",
+                output="screen",
+                parameters=[
+                    lidar_config,
+                    {"closest": use_closest, "draw_markers": draw_markers},
+                ],
+            ),
+            Node(
+                package="rplidar_ros",
+                executable="rplidar_composition",
+                output="screen",
+                parameters=[lidar_config, {"frame_id": lidar_frame}],
+            ),
+            Node(
+                package="rviz2",
+                executable="rviz2",
+                name="rviz2",
+                output="screen",
+                arguments=["-d", rviz_config],
+                condition=IfCondition(visualize),
+            ),
+        ]
+    )

toid_lidar/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>toid_lidar</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>eigen</depend>
+  <depend>nav2_util</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>toid_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>rplidar_ros</depend>
+
+
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

toid_lidar/params/lidar.yaml

@@ -0,0 +1,11 @@
+rplidar_node:
+  ros__parameters:
+    frame_id: "lidar_frame"
+    
+toid_lidar:
+  ros__parameters:
+    map_width: 3.0
+    map_height: 2.0
+    draw_makers: false
+    closest: true
+