finished toid_interaction node

docker-compose.yaml

@@ -1,6 +1,7 @@
 services:
   toid:
     image: localhost:5000/toid 
+    build: .
     container_name: toid
 
     privileged: true

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() {

toid_behaviors/include/toid_behaviors/move_coords.hpp

@@ -23,23 +23,31 @@ public:
     const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose & pose,
     const geometry_msgs::msg::Twist & vel) override;
 
+  double distanceToTarget(
+    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point, const double target_theta, bool backwards);
+
+  double velocityTarget(const double dist_left);
+  
+  bool collisionDetection(const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Pose &last_ok_pose);
+
   ResultStatus updateVel(
     const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
     geometry_msgs::msg::Twist & out_vel) override;
   
   virtual nav2_core::CostmapInfoType getResourceInfo() override {
-    return nav2_core::CostmapInfoType::NONE;
+    return nav2_core::CostmapInfoType::LOCAL;
   }
 
 protected:
 
-  SmoothControlLaw scl;
+  SmoothControlLaw scl_;
 
   //Goal
   geometry_msgs::msg::Pose target_pose_;
   double target_angle_;
   double target_sign_;
   bool backwards_;
+  unsigned char mode_;
 
   //State
   double last_speed_;

toid_behaviors/include/toid_behaviors/scl.hpp

@@ -19,13 +19,17 @@ public:
   double v_angular_max = 2.0;
 
   void egocentric_polar(
-    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
+    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
-    double & r, double & phi, double & delta);
+    bool backwards, double & r, double & phi, double & delta);
 
   double curvature(double r, double phi, double delta);
 
   void calculate_vel(
-    const geometry_msgs::msg::Pose & target,const geometry_msgs::msg::Pose & current,
+    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
     geometry_msgs::msg::Twist & out_speed, bool backwards = false);
+
+  void step(
+    const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt,
+    bool backwards = false);
 };
 }  // namespace toid

toid_behaviors/include/toid_behaviors/simple_move.hpp

@@ -6,6 +6,7 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "toid_msgs/action/simple_rotate.hpp"
+#include "toid_msgs/msg/rival.hpp"
 
 namespace toid
 {
@@ -14,6 +15,8 @@ template <typename ActionT>
 class SimpleMove : public nav2_behaviors::TimedBehavior<ActionT>
 {
 public:
+  using Rival = toid_msgs::msg::Rival;
+
   virtual void configureCB() {}
   virtual void cleanupCB() {}
 
@@ -34,6 +37,18 @@ public:
     nav2_util::declare_parameter_if_not_declared(
       node, "odom_topic", rclcpp::ParameterValue("/odom"));
     std::string odom_topic_name = node->get_parameter("odom_topic").as_string();
+
+    nav2_util::declare_parameter_if_not_declared(node, "robot_width", rclcpp::ParameterValue(0.30));
+    node->get_parameter("robot_width", robot_width_);
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, "robot_length", rclcpp::ParameterValue(0.30));
+    node->get_parameter("robot_length", robot_length_);
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, "rival_radius", rclcpp::ParameterValue(0.30));
+    node->get_parameter("rival_radius", rival_radius_);
+
     odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
       odom_topic_name, 1, [&](nav_msgs::msg::Odometry msg) {
         std::lock_guard lock(mut_);
@@ -52,12 +67,17 @@ public:
   void activate() override
   {
     nav2_behaviors::TimedBehavior<ActionT>::activate();
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock();
+    using namespace std::placeholders;
+    rivals_sub_ = node->create_subscription<Rival>(
+      "/dynamicObstacle", 1, std::bind(&SimpleMove<ActionT>::rival_cb, this, _1));
     activateCB();
   }
 
   void deactivate() override
   {
     nav2_behaviors::TimedBehavior<ActionT>::deactivate();
+    rivals_sub_.reset();
     deactivateCB();
   }
 
@@ -105,12 +125,51 @@ public:
     return r;
   }
 
+  bool check_rival_collision(geometry_msgs::msg::Pose2D & pose)
+  {
+    if (!rivals_) {
+      return false;
+    }
+    const double cosp = std::cos(pose.theta);
+    const double sinp = std::sin(pose.theta);
+    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;
+
+      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);
+      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.30;
+
+  Rival::SharedPtr rivals_;
+  rclcpp::Subscription<Rival>::SharedPtr rivals_sub_;
 };
 
 }  // namespace toid

toid_behaviors/include/toid_behaviors/simple_rotate.hpp

@@ -2,6 +2,7 @@
 
 #include "toid_behaviors/simple_move.hpp"
 #include "toid_msgs/action/simple_rotate.h"
+#include "toid_msgs/msg/rival.hpp"
 
 namespace toid
 {
@@ -23,17 +24,25 @@ public:
   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 {
+  virtual nav2_core::CostmapInfoType getResourceInfo() override
-    return nav2_core::CostmapInfoType::NONE;
+  {
+    return nav2_core::CostmapInfoType::LOCAL;
   }
 
-protected:
+  void calc_err_and_sign(
+    double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign);
 
+  double check_space(const geometry_msgs::msg::Pose pose, const double e, const double sign);
+
+  double calc_speed(const double err, const double sign, const double angular_speed);
+
+protected:
   //Goal
   double target_angle_;
   double min_turn_angle_;
   double initial_direction_;
+  unsigned char mode_;
 
   //State
   double angular_speed_;
@@ -45,6 +54,7 @@ protected:
   double min_angular_speed_;
   double max_angular_accel_;
   double max_angular_decel_;
+  double lookahead_;
 };
 
 }  // namespace toid

toid_behaviors/src/move_coords.cpp

@@ -69,43 +69,87 @@ ResultStatus MoveCoords::onStart(
   target_sign_ = backwards_ ? -1.0 : 1.0;
   max_vel_speed_ = command->max_speed;
 
-  if(command->max_speed == 0) {
+  if (command->max_speed == 0) {
     auto node = node_.lock();
     node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
   }
 
-  scl.k_phi = k_phi_;
+  mode_ = command->mode;
-  scl.k_delta = k_delta_;
+
-  scl.bbeta = beta_;
+  scl_.k_phi = k_phi_;
-  scl.lam = lambda_;
+  scl_.k_delta = k_delta_;
-  scl.slowdown_radius = slowdown_radius_;
+  scl_.bbeta = beta_;
-  scl.v_angular_max = max_angular_speed_;
+  scl_.lam = lambda_;
-  scl.v_linear_min = min_vel_speed_;
+  scl_.slowdown_radius = slowdown_radius_;
-  scl.v_linear_max = max_vel_speed_;
+  scl_.v_angular_max = max_angular_speed_;
+  scl_.v_linear_min = min_vel_speed_;
+  scl_.v_linear_max = max_vel_speed_;
 
   last_speed_ = vel.angular.x;
 
   return ResultStatus{Status::SUCCEEDED};
 }
 
+double MoveCoords::distanceToTarget(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point,
+  const double target_theta, bool backwards)
+{
+  const double dx = target_point.x - pose.position.x;
+  const double dy = target_point.y - pose.position.y;
+  const double target_sign = backwards ? -1.0 : 1.0;
+
+  return target_sign * (dx * cos(target_theta) + dy * sin(target_theta));
+}
+
+double MoveCoords::velocityTarget(const double dist_left)
+{
+  const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_;
+  const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_;
+
+  double vel = max_vel_speed_;
+  double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
+  vel = std::min(vel, max_vel_to_stop);
+  return std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+}
+
+bool MoveCoords::collisionDetection(
+  const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Pose & last_ok_pose)
+{
+  const int samples = static_cast<int>(0.5 / control_duration_);
+  geometry_msgs::msg::Pose current_pose = pose;
+  last_ok_pose = pose;
+  const bool check_map = !(mode_ & MoveAction::Goal::IGNORE_OBSTACLES);
+  for (int i = 0; i < samples; i++) {
+    scl_.step(target_pose_, current_pose, control_duration_, backwards_);
+
+    geometry_msgs::msg::Pose2D p;
+    p.x = current_pose.position.x;
+    p.y = current_pose.position.y;
+    p.theta = tf2::getYaw(current_pose.orientation);
+
+    if (check_map && !local_collision_checker_->isCollisionFree(p, i == 0)) {
+      return true;
+    }
+
+    if (check_rival_collision(p)) {
+      return true;
+    }
+
+    last_ok_pose = current_pose;
+    const double dist_left =
+      distanceToTarget(current_pose, target_pose_.position, target_angle_, backwards_);
+    if (dist_left < 0.005) {
+      return false;
+    }
+  }
+  return false;
+}
+
 ResultStatus MoveCoords::updateVel(
   const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
   geometry_msgs::msg::Twist & out_vel)
 {
-  const double current_yaw = tf2::getYaw(pose.orientation);
+  double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_);
-  double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
-
-  if (backwards_) {
-    angle_dist = angles::two_pi_complement(angle_dist);
-  }
-
-  const double dx = target_pose_.position.x - pose.position.x;
-  const double dy = target_pose_.position.y - pose.position.y;
-
-  const double dist_left = target_sign_ * (dx * cos(target_angle_) + dy * sin(target_angle_));
-
-  const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_;
-  const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_;
 
   if (dist_left <= 0.001) {
     out_vel.linear.x = 0;
@@ -113,30 +157,48 @@ ResultStatus MoveCoords::updateVel(
     return ResultStatus{Status::SUCCEEDED};
   }
 
-  double vel = max_vel_speed_;
+  const double current_yaw = tf2::getYaw(pose.orientation);
-  double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
+  double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
-  vel = std::min(vel, max_vel_to_stop);
 
+  geometry_msgs::msg::Pose last_ok_pose;
+  if (collisionDetection(pose, last_ok_pose)) {
+    dist_left = distanceToTarget(
+      pose, last_ok_pose.position, tf2::getYaw(last_ok_pose.orientation), backwards_);
+    if (dist_left <= 0.02) {
+      out_vel.linear.x = 0;
+      out_vel.angular.z = 0;
+    } else {
+      scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
+      scl_.calculate_vel(last_ok_pose, pose, out_vel, backwards_);
+    }
+
+    last_speed_ = out_vel.linear.x;
+
+    RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
+    RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max);
+
+    return ResultStatus{Status::RUNNING};
+  }
 
   if (dist_left <= 0.02) {
-    out_vel.linear.x = std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+    out_vel.linear.x = velocityTarget(dist_left);
-    out_vel.angular.z = std::clamp(target_sign_ * kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
+    out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
     last_speed_ = out_vel.linear.x;
     return ResultStatus{Status::RUNNING};
   }
 
-  scl.v_linear_max = target_sign_ * std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+  scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
-  scl.calculate_vel(target_pose_, pose, out_vel, backwards_);
+  scl_.calculate_vel(target_pose_, pose, out_vel, backwards_);
+
   last_speed_ = out_vel.linear.x;
 
-  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); 
+  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
-  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl.v_linear_max); 
+  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max);
 
   return ResultStatus{Status::RUNNING};
 }
 
 }  // namespace toid
 
-
 #include "pluginlib/class_list_macros.hpp"
 PLUGINLIB_EXPORT_CLASS(toid::MoveCoords, nav2_core::Behavior);

toid_behaviors/src/scl.cpp

@@ -3,20 +3,21 @@
 #include <cmath>
 
 #include "angles/angles.h"
+#include "nav2_util/geometry_utils.hpp"
 #include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
-
+namespace toid
-namespace toid {
+{
 void SmoothControlLaw::calculate_vel(
   const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
-  geometry_msgs::msg::Twist &out_speed, bool backwards)
+  geometry_msgs::msg::Twist & out_speed, bool backwards)
 {
   double r, phi, delta;
   egocentric_polar(target, current, backwards, r, phi, delta);
 
   double curvature = this->curvature(r, phi, delta);
-  curvature = backwards? -curvature : curvature;
+  curvature = backwards ? -curvature : curvature;
 
   double v = v_linear_max / (1.0 + bbeta * std::pow(fabs(curvature), lam));
 
@@ -44,8 +45,8 @@ double SmoothControlLaw::curvature(double r, double phi, double delta)
 }
 
 void SmoothControlLaw::egocentric_polar(
-  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards, double & r,
+  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
-  double & phi, double & delta)
+  double & r, double & phi, double & delta)
 {
   const double dx = target.position.x - current.position.x;
   const double dy = target.position.y - current.position.y;
@@ -59,4 +60,20 @@ void SmoothControlLaw::egocentric_polar(
   delta = angles::normalize_angle(ctheta + los);
 }
 
-}
+void SmoothControlLaw::step(
+  const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt,
+  bool backwards)
+{
+  geometry_msgs::msg::Twist twist;
+  calculate_vel(target, current, twist, backwards);
+
+  double theta = tf2::getYaw(current.orientation);
+  double dx = twist.linear.x * dt * cos(theta);
+  double dy = twist.linear.x * dt * sin(theta);
+  current.orientation =
+    nav2_util::geometry_utils::orientationAroundZAxis(theta + twist.angular.z * dt);
+  current.position.x += dx;
+  current.position.y += dy;
+}
+
+}  // namespace toid

toid_behaviors/src/simple_rotate.cpp

@@ -4,6 +4,7 @@
 
 #include "angles/angles.h"
 #include "tf2/convert.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 namespace toid
 {
@@ -30,6 +31,10 @@ void SimpleRotateBehavior::configureCB()
   nav2_util::declare_parameter_if_not_declared(
     node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(4.0));
   node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".lookahead", rclcpp::ParameterValue(0.5));
+  node->get_parameter(behavior_name_ + ".lookahead", lookahead_);
 }
 
 ResultStatus SimpleRotateBehavior::onStart(
@@ -40,12 +45,19 @@ ResultStatus SimpleRotateBehavior::onStart(
   min_turn_angle_ = abs(command->min_angle);
   initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0;
   max_angular_speed_ = command->max_speed;
+  mode_ = command->mode;
 
-  if(command->max_speed == 0) {
+  if (command->max_speed == 0) {
     auto node = node_.lock();
     node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
   }
 
+  geometry_msgs::msg::Pose2D pose2d;
+  pose2d.x = pose.position.x;
+  pose2d.y = pose.position.y;
+  pose2d.theta = initial_direction_;
+  local_collision_checker_->isCollisionFree(pose2d, true);
+
   last_angle_ = tf2::getYaw(pose.orientation);
 
   angular_speed_ = vel.angular.z;
@@ -53,43 +65,108 @@ ResultStatus SimpleRotateBehavior::onStart(
   return ResultStatus{Status::SUCCEEDED};
 }
 
+void SimpleRotateBehavior::calc_err_and_sign(
+  double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign)
+{
+  err = angles::shortest_angular_distance(current_yaw, target_angle_);
+  sign = (err < 0) ? -1.0 : 1.0;
+  err = std::abs(err);
+
+  if (min_turn_angle > 0.0) {
+    const double angle_change = angles::shortest_angular_distance(last_angle, current_yaw);
+    min_turn_angle = std::max(0.0, min_turn_angle - initial_direction_ * angle_change);
+    err = std::max(initial_direction_ * sign * err, 0.0);
+    err = std::max(min_turn_angle, err);
+    sign = initial_direction_;
+  }
+}
+
+double SimpleRotateBehavior::calc_speed(
+  const double err, const double sign, const double angular_speed)
+{
+  const double upper_vel_ = angular_speed + max_angular_accel_ * control_duration_;
+  const double lower_vel_ = angular_speed - max_angular_accel_ * control_duration_;
+
+  const double speed_until_overshoot = std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
+
+  const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
+  const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);
+  return speed;
+}
+
 ResultStatus SimpleRotateBehavior::updateVel(
   const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
   geometry_msgs::msg::Twist & out_vel)
 {
   const double current_yaw = tf2::getYaw(pose.orientation);
-  const double angle_change = angles::shortest_angular_distance(last_angle_ , current_yaw);
+  double min_turn_angle = min_turn_angle_;
-  last_angle_ = current_yaw;
+  double angular_speed = angular_speed_;
+  double err, sign;
 
-  double err = angles::shortest_angular_distance(current_yaw, target_angle_);
+  calc_err_and_sign(last_angle_, current_yaw, min_turn_angle, err, sign);
-  double sign = (err < 0)? -1.0 : 1.0;
-  err = std::abs(err);
 
-  if (min_turn_angle_ > 0.0) {
+  if (!(mode_ & RotateAction::Goal::IGNORE_OBSTACLES)) {
-    min_turn_angle_ = std::max(0.0, min_turn_angle_ - initial_direction_ * angle_change);
+    err = check_space(pose, err, sign);
-    err = std::max( initial_direction_ * sign * err, 0.0);
-    err = std::max(min_turn_angle_, err);
-    sign = initial_direction_;
   }
 
-  const double upper_vel_ = angular_speed_ + max_angular_accel_ * control_duration_;
+  double speed = 0.0;
-  const double lower_vel_ = angular_speed_ - max_angular_accel_ * control_duration_;
 
-  const double speed_until_overshoot =
+  if (err != 0.0) {
-    std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
+    speed = calc_speed(err, sign, angular_speed);
-
+  }
-  const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
-  const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);
 
   if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.01) {
     return ResultStatus{Status::SUCCEEDED};
   }
 
+  min_turn_angle_ = min_turn_angle;
+  last_angle_ = current_yaw;
   angular_speed_ = speed;
   out_vel.angular.z = speed;
   return ResultStatus{Status::RUNNING};
 }
 
+double SimpleRotateBehavior::check_space(
+  const geometry_msgs::msg::Pose pose, const double e, const double sign)
+{
+  geometry_msgs::msg::Pose2D pose2d;
+  pose2d.x = pose.position.x;
+  pose2d.y = pose.position.y;
+  double initial_theta = tf2::getYaw(pose.orientation);
+  pose2d.theta = initial_theta;
+  const double step_size = 0.1;
+  const double err = std::min(e, 1.0);
+  const bool check_map = !(mode_ & RotateAction::Goal::IGNORE_OBSTACLES);
+
+  for (int i = 1; i < err / step_size; i++) {
+    pose2d.theta += sign * step_size;
+
+    if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
+      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+      return step_size * (i - 1);
+    }
+
+    if (check_rival_collision(pose2d)) {
+      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+      return step_size * (i - 1);
+    }
+  }
+
+  pose2d.theta = initial_theta + sign * err;
+
+  if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+    return step_size * ((int)(err / step_size));
+  }
+
+  if (check_rival_collision(pose2d)) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+    return step_size * ((int)(err / step_size));
+  }
+
+  return e;
+}
+
 }  // namespace toid
 
 #include "pluginlib/class_list_macros.hpp"

toid_bt/behavior_trees/behavior1.xml

@@ -1,17 +1,111 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
+  <BehaviorTree ID="seq">
+    <Sequence>
+      <Seq1 service_name=""/>
+      <Seq2 text="0101"
+            service_name=""/>
+      <Seq3 service_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="seq1">
+    <Sequence>
+      <ZeroOdom service_name=""/>
+      <MovePointSimple x="1.05"
+                       y="0"
+                       theta="0"
+                       max_speed="0.300000"
+                       backwards="false"
+                       action_name=""/>
+      <MovePointSimple x="0.76"
+                       y="0.18"
+                       theta="-90"
+                       max_speed="0.250000"
+                       backwards="true"
+                       action_name=""/>
+      <TranslateX x="0.5"
+                  max_speed="0.300000"
+                  action_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
   <BehaviorTree ID="test_1">
     <Sequence>
-      <RotateSimple angle="90"
+      <DetectStuck timeout="1.000000">
-                    min_angle="0.000000"
+        <RotateTowards x="0.4"
-                    action_name=""/>
+                       y="0.0"
-      <Sleep msec="1000"/>
+                       max_speed="0.000000"
-      <RotateSimple angle="0"
+                       min_angle="0.000000"
-                    min_angle="0.000000"
+                       action_name=""/>
-                    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>
+    <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="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>
+    <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="ZeroOdom">
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
+  </TreeNodesModel>
 
 </root>

toid_bt/behavior_trees/calibration_routines.xml

@@ -1,45 +1,116 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
-  <BehaviorTree ID="wheel_ratio_calibration">
+  <BehaviorTree ID="track_calib">
     <Sequence>
-      <Delay delay_msec="5000">
+      <SetBlackboard value="0.265"
-        <RotateSimple angle="0"
+                     output_key="width"/>
-                      max_speed="0.000000"
+      <Sleep msec="1000"/>
-                      min_angle="270"
+      <ZeroOdom service_name=""/>
-                      action_name=""/>
+      <RotateTowards x="0.4"
-      </Delay>
+                     y="0.0"
+                     max_speed="0.000000"
+                     min_angle="0.000000"
+                     action_name=""/>
       <Sequence>
-        <MovePointSimple x="1.0"
+        <Sleep msec="1000"/>
+        <MovePointSimple x="0.4"
                          y="0"
                          theta="0"
                          max_speed="0.10000"
+                         backwards="false"
                          action_name=""/>
-        <RotateSimple angle="180"
+        <Sleep msec="500"/>
+        <RotateSimple angle="0"
                       max_speed="0.500000"
+                      min_angle="270"
+                      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="1"
+                new_width="{width}"
+                service_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="wheel_ratio_calibration">
+    <Sequence>
+      <Sleep msec="5000"/>
+      <ZeroOdom service_name=""/>
+      <Sleep msec="1000"/>
+      <Sequence>
+        <MovePointSimple x="1.1"
+                         y="0"
+                         theta="0"
+                         max_speed="0.10000"
+                         backwards="false"
+                         action_name=""/>
+        <Sleep msec="1000"/>
+        <RotateSimple angle="180"
+                      max_speed="0.300000"
                       min_angle="10"
                       action_name=""/>
-        <MovePointSimple x="0.4"
+        <Sleep msec="500"/>
+        <MovePointSimple x="0.35"
                          y="0"
                          theta="180"
                          max_speed="0.100000"
+                         backwards="false"
                          action_name=""/>
         <RotateSimple angle="0"
-                      max_speed="0.500000"
+                      max_speed="0.300000"
                       min_angle="-10"
                       action_name=""/>
       </Sequence>
-      <TranslateX x="-0.5"
+      <ForceSuccess>
-                  max_speed="0.050000"
+        <DetectStuck timeout="1.000000">
-                  action_name=""/>
+          <MovePointSimple x="-0.2"
-      <RotateSimple angle="0"
+                           y="0"
-                    max_speed="0.000000"
+                           theta="0"
-                    min_angle="-270"
+                           max_speed="0.05"
-                    action_name=""/>
+                           backwards="true"
+                           action_name=""/>
+        </DetectStuck>
+      </ForceSuccess>
+      <Sleep msec="1000"/>
+      <EndCalib service_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="wheel_size">
+    <Sequence>
+      <Sleep msec="1000"/>
+      <ZeroOdom service_name=""/>
+      <Sleep msec="1000"/>
+      <MovePointSimple x="1.200"
+                       y="0"
+                       theta="0"
+                       max_speed="0.250000"
+                       backwards="false"
+                       action_name=""/>
     </Sequence>
   </BehaviorTree>
 
   <!-- Description of Node Models (used by Groot) -->
   <TreeNodesModel>
+    <Decorator ID="DetectStuck">
+      <input_port name="timeout"
+                  default="1.000000"
+                  type="double"/>
+    </Decorator>
+    <Action ID="EndCalib">
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
     <Action ID="MovePointSimple">
       <input_port name="x"
                   type="double"/>
@@ -50,6 +121,9 @@
       <input_port name="max_speed"
                   default="0.000000"
                   type="double"/>
+      <input_port name="backwards"
+                  default="false"
+                  type="bool"/>
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
     </Action>
@@ -65,15 +139,35 @@
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
     </Action>
-    <Action ID="TranslateX">
+    <Action ID="RotateTowards">
       <input_port name="x"
                   type="double"/>
+      <input_port name="y"
+                  type="double"/>
       <input_port name="max_speed"
                   default="0.000000"
                   type="double"/>
+      <input_port name="min_angle"
+                  default="0.000000"
+                  type="double"/>
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
     </Action>
+    <Action ID="SetWidth">
+      <input_port name="width"
+                  type="double"/>
+      <input_port name="count"
+                  default="1"
+                  type="int"/>
+      <output_port name="new_width"
+                   type="double"/>
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
+    <Action ID="ZeroOdom">
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
   </TreeNodesModel>
 
 </root>

toid_bt/behavior_trees/toid_behaviors.btproj

@@ -4,6 +4,9 @@
     <include path="calibration_routines.xml"/>
     <!-- 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>
@@ -12,6 +15,7 @@
             <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">
@@ -27,6 +31,16 @@
             <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>
         <Action ID="SetWidth">
             <input_port name="width" type="double"/>
             <input_port name="count" default="1" type="int"/>

toid_bt/include/toid_bt/plugins/end_calib_action.hpp

@@ -33,7 +33,7 @@ public:
   bool setRequest(typename Request::SharedPtr & request) override
   {
     auto width = getInput<double>("width").value();
-    auto count = getInput<double>("count").value();
+    double count = getInput<int>("count").value();
 
     geometry_msgs::msg::PoseStamped pose;
     get_pose(pose);
@@ -42,6 +42,8 @@ public:
     double new_width = width * (1 + (theta / (2 * M_PI * count)));
     request->data = new_width;
 
+    RCLCPP_INFO(logger(), "width is %lf", new_width);
+
     setOutput("new_width", new_width);
     return true;
   }

toid_bt/include/toid_bt/plugins/move_coords_action.hpp

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

toid_bt/include/toid_bt/plugins/rotate_action.hpp

@@ -40,6 +40,7 @@ public:
     goal.max_speed = max_speed.value();
     goal.angle = angles::from_degrees(goal.angle);
     goal.min_angle = angles::from_degrees(goal.min_angle);
+    goal.mode = 1;
     return true;
   }
 

toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp

@@ -44,6 +44,7 @@ public:
     goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x);
     goal.min_angle = angles::from_degrees(min_angle);
     goal.max_speed = max_speed;
+    goal.mode = 1;
 
     return true;
   }

toid_bt/include/toid_bt/plugins/send_text_action.hpp

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

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/src/bt_executor.cpp

@@ -9,6 +9,8 @@
 #include "toid_bt/plugins/move_coords_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/stuck_detector_decorator.hpp"
 #include "toid_bt/plugins/translate_x_action.hpp"
 
 namespace toid
@@ -19,9 +21,9 @@ TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts)
   /* 
   executeRegistration();
   std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
+  */
   tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
-  */
 
   nav2_util::declare_parameter_if_not_declared(
     node(), "base_frame", rclcpp::ParameterValue("base_footprint"));
@@ -60,6 +62,16 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
 
   factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
 
+  factory.registerNodeType<toid::EmptySrvNode>("Seq1", BT::RosNodeParams(nh, "/sequence1"));
+
+  BT::RosNodeParams service_params(nh, "/sequence2");
+  service_params.server_timeout = std::chrono::seconds(15);
+  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params);
+
+  factory.registerNodeType<toid::EmptySrvNode>("Seq3", BT::RosNodeParams(nh, "/sequence3"));
+
+  factory.registerNodeType<toid::StuckDetectorNode>("DetectStuck", get_pose);
+
   std::cout << describeCustomNodes() << std::endl;
 }
 

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

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,97 @@
+import rclpy
+from rclpy.node import Node
+from std_srvs.srv import Empty
+
+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.zupcanik import ZupcanikAction
+from toid_msgs.srv import SendString
+
+class InteracitionNode(Node):
+    step: int = 0
+    def __init__(self):
+        super().__init__('ToidInteractionNode')
+
+        self.find_sigma()
+
+        self.srv = self.create_service(
+            Empty,
+            '/sequence1',
+            self.sequence1_cb
+        )
+
+        self.srv = self.create_service(
+            SendString,
+            '/sequence2',
+            self.sequence2_cb
+        )
+
+        self.srv = self.create_service(
+            Empty,
+            '/sequence3',
+            self.sequence3_cb
+        )
+
+
+
+        self.get_logger().info("Service 'sequence1' ready.")
+
+    def find_sigma(self):
+        for port_info in list_ports.comports():
+            if port_info.vid == 0x18a6 and port_info.pid == 0x55d3:
+                break
+
+        print(port_info.device)
+        self.st_motor_device_name = port_info.device
+
+    def sequence1_cb(self, request, response):
+        if(self.step != 0):
+            return Empty.Response()
+        okreni(5)
+        zupcanik = ZupcanikAction(self.st_motor_device_name)
+        zupcanik.zupcanik(1, -1010, 25)
+        self.step = 1
+        return response
+
+    def sequence2_cb(self, request : SendString.Request, response : SendString.Response):
+        if(self.step != 1):
+            return Empty.Response()
+        zidovi = ZidoviAction(self.st_motor_device_name)
+        zidovi.beli_zid(1)
+        zidovi.plavi_zid(1)
+
+        okreni_niz(request.text)
+
+        zidovi.plavi_zid(0, TargetPos=150)
+        zidovi.beli_zid(0, TargetPos=450)
+        self.step = 2
+        return response
+
+    def sequence3_cb(self, request, response):
+        if(self.step != 2):
+            return Empty.Response()
+
+        zupcanik = ZupcanikAction(self.st_motor_device_name)
+        zidovi = ZidoviAction(self.st_motor_device_name) 
+
+        zupcanik.zupcanik(1, 1010, 25)
+        zidovi.plavi_zid(0, TargetPos=150)
+        zidovi.beli_zid(0, TargetPos=150)
+        okreni(5)
+        self.step = 0
+        return response
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = InteracitionNode()
+    rclpy.spin(node)
+
+    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,140 @@
+#!/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 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/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 = "50443405C8C3B21C"
+
+
+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_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,68 @@
+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
+    

toid_lidar/rviz/conf.rviz

@@ -0,0 +1,175 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.71659916639328
+    Tree Height: 1144
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: Marker
+      Namespaces:
+        "": true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /rivalMaker
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 47
+      Min Color: 0; 0; 0
+      Min Intensity: 47
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Angle: -0.24500009417533875
+      Class: rviz_default_plugins/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 152.6725311279297
+      Target Frame: <Fixed Frame>
+      Value: TopDownOrtho (rviz_default_plugins)
+      X: 0
+      Y: 0
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1662
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f000000574fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000005740000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b00000574fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000005740000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000b400000005efc0100000002fb0000000800540069006d0065010000000000000b400000048700fffffffb0000000800540069006d00650100000000000004500000000000000000000007dd0000057400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2880
+  X: 0
+  Y: 64

toid_lidar/src/toid_lidar.cpp

@@ -0,0 +1,9 @@
+#include "rclcpp/rclcpp.hpp"
+#include "toid_lidar/toid_lidar_node.hpp"
+
+int main(const int argc,const char** argv) {
+    rclcpp::init(argc,argv);
+    rclcpp::spin(std::make_shared<toid::ToidRivalDetect>());
+    rclcpp::shutdown();
+    return 0;
+}

toid_lidar/src/toid_lidar_node.cpp

@@ -0,0 +1,149 @@
+#include "toid_lidar/toid_lidar_node.hpp"
+
+#include "Eigen/Eigen"
+#include "Eigen/Geometry"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "tf2/convert.hpp"
+#include "tf2_eigen/tf2_eigen.hpp"
+
+namespace toid
+{
+
+ToidRivalDetect::ToidRivalDetect() : rclcpp::Node("RivalDetect")
+{
+  scan_sub_ = create_subscription<LaserScan>(
+    "scan", rclcpp::QoS(1), [this](LaserScan::ConstSharedPtr msg) { this->process_scan(msg); });
+  rival_pub_ = create_publisher<Rival>("/dynamicObstacle", rclcpp::QoS(1));
+  marker_pub_ = create_publisher<Marker>("/rivalMaker", rclcpp::QoS(1));
+
+  tf_buf_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buf_);
+
+  clock_ = this->get_clock();
+  logger_ = this->get_logger();
+
+  nav2_util::declare_parameter_if_not_declared(this, "global_frame", rclcpp::ParameterValue("map"));
+  this->get_parameter("global_frame", global_frame_);
+
+  nav2_util::declare_parameter_if_not_declared(this, "map_width", rclcpp::ParameterValue(3.0));
+  this->get_parameter("map_width", map_width_);
+
+  nav2_util::declare_parameter_if_not_declared(this, "map_height", rclcpp::ParameterValue(2.0));
+  this->get_parameter("map_height", map_height_);
+
+  nav2_util::declare_parameter_if_not_declared(this, "draw_markers", rclcpp::ParameterValue(false));
+  this->get_parameter("draw_markers", visualize_);
+
+  nav2_util::declare_parameter_if_not_declared(this, "closest", rclcpp::ParameterValue(false));
+  this->get_parameter("closest", closest_);
+}
+
+void ToidRivalDetect::process_scan(LaserScan::ConstSharedPtr msg)
+{
+  Eigen::Isometry3d pose;
+
+  geometry_msgs::msg::TransformStamped transform;
+
+  try {
+    transform = tf_buf_->lookupTransform(global_frame_, msg->header.frame_id, msg->header.stamp);
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_WARN_THROTTLE(
+      this->get_logger(), *this->get_clock(), 1000, "TF Link missing: %s", e.what());
+    return;
+  }
+
+  Eigen::Isometry3d iso = tf2::transformToEigen(transform);
+
+  double min_dist = INFINITY;
+
+  toid_msgs::msg::Rival rival;
+  rival.header.frame_id = global_frame_;
+  rival.header.stamp = msg->header.stamp;
+
+  auto marker_loan = marker_pub_->borrow_loaned_message();
+  Marker & marker = marker_loan.get();
+  if(visualize_) {
+    marker.header = rival.header;
+    marker.scale.x = 0.1;
+    marker.scale.y = 0.1;
+    marker.scale.z = 0.1;
+    marker.type = Marker::SPHERE_LIST;
+    marker.frame_locked = true;
+    marker.lifetime.sec = 1;
+    marker.color.a = 1.0;
+    marker.color.g = 1.0;
+    marker.id = 1;
+  }
+
+  Eigen::Vector3d accum(0, 0, 0);
+  Eigen::Vector3d start_point(0, 0, 0);
+
+  int points = 0;
+
+  for (size_t i = 0; i < msg->ranges.size(); ++i) {
+    double range = msg->ranges[i];
+    double intensity = msg->intensities[i];
+
+    if (range < msg->range_min || range > msg->range_max) continue;
+    if (intensity < 35.0) continue;
+
+    double angle = msg->angle_min + (i * msg->angle_increment);
+
+    Eigen::Vector3d local_point(range * std::cos(angle), range * std::sin(angle), 0.0);
+    Eigen::Vector3d map_point = iso * local_point;
+
+    if (
+      std::fabs(map_point.x()) < map_width_ / 2.0 && std::fabs(map_point.y()) < map_height_ / 2.0) {
+      if ((start_point - map_point).dot(start_point - map_point) < 0.025) {
+        accum += map_point;
+        points++;
+      } else {
+        if (points != 0 && !closest_) {
+          geometry_msgs::msg::Point p;
+          accum /= points;
+          tf2::convert(accum, p);
+          if(visualize_){
+            marker.points.push_back(p);
+          }
+          rival.point.push_back(p);
+        }
+        points = 1;
+        accum = map_point;
+        start_point = map_point;
+      }
+
+      if(range < min_dist && closest_) {
+        min_dist = range;
+        if(rival.point.empty()) {
+          if(visualize_){
+            marker.points.emplace_back();
+          }
+          rival.point.emplace_back();
+        }
+        tf2::convert(map_point, rival.point.front());
+        if(visualize_) {
+            tf2::convert(map_point, marker.points.front());
+        }
+      }
+    }
+  }
+
+  if (points != 0 && !closest_) {
+    geometry_msgs::msg::Point p;
+    accum /= points;
+    tf2::convert(accum, p);
+    if(visualize_){
+      marker.points.push_back(p);
+    }
+    rival.point.push_back(p);
+  }
+
+  if(visualize_) {
+    marker_pub_->publish(std::move(marker_loan));
+  }
+  rival_pub_->publish(rival);
+}
+
+}  // namespace toid

toid_msgs/CMakeLists.txt

@@ -9,11 +9,17 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
+find_package(geometry_msgs REQUIRED)
+
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/ActiveElements.msg"
+  "msg/Rival.msg"
   "srv/SendDouble.srv"
+  "srv/SendString.srv"
   "action/SimpleMoveCoords.action"
   "action/SimpleRotate.action"
   "action/SimpleTranslateX.action"
+  DEPENDENCIES geometry_msgs
 )
 
 ament_package()

toid_msgs/msg/ActiveElements.msg

@@ -0,0 +1,2 @@
+# Activate/deactivate game elements
+bool[] active

toid_msgs/msg/Rival.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+geometry_msgs/Point[] point

toid_msgs/package.xml

@@ -10,6 +10,8 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>
 

toid_msgs/srv/SendString.srv

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

toid_navigation/params/toid_costmap_params.yaml

@@ -1,14 +1,14 @@
 global_costmap:
   ros__parameters:
-    update_frequency: 1.0
+    update_frequency: 10.0
-    publish_frequency: 1.0
+    publish_frequency: 10.0
     global_frame: map
     robot_base_frame: base_link
     robot_radius: 0.17
     resolution: 0.01
     track_unknown_space: false
     rolling_window: false
-    plugins: ["static_layer", "inflation_layer"]
+    plugins: ["static_layer", "game_element_layer", "rival_layer", "inflation_layer"]
     static_layer:
       plugin: "nav2_costmap_2d::StaticLayer"
       map_subscribe_transient_local: True
@@ -16,6 +16,12 @@ global_costmap:
       plugin: "nav2_costmap_2d::InflationLayer"
       cost_scaling_factor: 3.0
       inflation_radius: 0.23
+    game_element_layer:
+      plugin: "toid::GameElementLayer"
+      extra_elements: ["blue"]
+    rival_layer:
+      plugin: "toid::RivalLayer"
+      rival_size: 0.15
     always_send_full_costmap: True
 
 lifecycle_manager: