finished toid_interaction node

toid_bt/behavior_trees/behavior1.xml

@@ -1,5 +1,14 @@
 <?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=""/>
@@ -70,6 +79,20 @@
       <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"/>

toid_bt/behavior_trees/calibration_routines.xml

@@ -26,14 +26,14 @@
                       action_name=""/>
       </Sequence>
       <ForceSuccess>
-        <Timeout msec="13000">
+        <DetectStuck timeout="1.000000">
           <MovePointSimple x="-1.0"
                            y="0"
                            theta="0"
                            max_speed="0.070000"
                            backwards="true"
                            action_name=""/>
-        </Timeout>
+        </DetectStuck>
       </ForceSuccess>
       <SetWidth width="{width}"
                 count="1"
@@ -72,14 +72,14 @@
                       action_name=""/>
       </Sequence>
       <ForceSuccess>
-        <Timeout msec="9000">
+        <DetectStuck timeout="1.000000">
           <MovePointSimple x="-0.2"
                            y="0"
                            theta="0"
                            max_speed="0.05"
                            backwards="true"
                            action_name=""/>
-        </Timeout>
+        </DetectStuck>
       </ForceSuccess>
       <Sleep msec="1000"/>
       <EndCalib service_name=""/>
@@ -102,6 +102,11 @@
 
   <!-- Description of Node Models (used by Groot) -->
   <TreeNodesModel>
+    <Decorator ID="DetectStuck">
+      <input_port name="timeout"
+                  default="1.000000"
+                  type="double"/>
+    </Decorator>
     <Action ID="EndCalib">
       <input_port name="service_name"
                   type="std::string">Service name</input_port>

toid_bt/behavior_trees/toid_behaviors.btproj

@@ -5,45 +5,55 @@
     <!-- Description of Node Models (used by Groot) -->
     <TreeNodesModel>
         <Decorator ID="DetectStuck">
-            <input_port name="timeout" type="double" default="1.000000"/>
+            <input_port name="timeout" default="1.000000" type="double"/>
         </Decorator>
         <Action ID="EndCalib">
-            <input_port name="service_name" type="std::string" default="">Service name</input_port>
+            <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
         <Action ID="MovePointSimple">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
             <input_port name="theta" type="double"/>
-            <input_port name="max_speed" type="double" default="0.000000"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
-            <input_port name="backwards" type="bool" default="false"/>
+            <input_port name="backwards" default="false" type="bool"/>
-            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
         </Action>
         <Action ID="RotateSimple">
             <input_port name="angle" type="double"/>
-            <input_port name="max_speed" type="double" default="0.000000"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
-            <input_port name="min_angle" type="double" default="0.000000"/>
+            <input_port name="min_angle" default="0.000000" type="double"/>
-            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
         </Action>
         <Action ID="RotateTowards">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
-            <input_port name="max_speed" type="double" default="0.000000"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
-            <input_port name="min_angle" type="double" default="0.000000"/>
+            <input_port name="min_angle" default="0.000000" type="double"/>
-            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+            <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" type="int" default="1"/>
+            <input_port name="count" default="1" type="int"/>
             <output_port name="new_width" type="double"/>
-            <input_port name="service_name" type="std::string" default="">Service name</input_port>
+            <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
         <Action ID="TranslateX">
             <input_port name="x" type="double"/>
-            <input_port name="max_speed" type="double" default="0.000000"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
-            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
         </Action>
         <Action ID="ZeroOdom">
-            <input_port name="service_name" type="std::string" default="">Service name</input_port>
+            <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
     </TreeNodesModel>
 </root>

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

@@ -9,6 +9,7 @@
 #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"
 
@@ -61,6 +62,14 @@ 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_interaction/setup.py

@@ -24,7 +24,8 @@ setup(
     },
     entry_points={
         'console_scripts': [
-            'sequence = toid_interaction.mechanism.sekvenca_2026:main'
+            'sequence = toid_interaction.mechanism.sekvenca_2026:main',
+            'node = toid_interaction.interaction_node:main'
         ],
     },
 )

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/sekvenca_2026.py

@@ -2,26 +2,24 @@
 # 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 *
+from .zidovi_load import ZidoviAction
-from .zuocanik import *
+from .zupcanik import ZupcanikAction
 import time
 import serial
 import serial.tools.list_ports as list_ports
-import platform
 
-SERIAL_ID = '503359270A70619F'
+SERIAL_ID = "50443405C8C3B21C"
 
 
 def okreni(i):
- # Funkci
     for port_info in list_ports.comports():
-        print(port_info.name)
         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)
@@ -29,36 +27,41 @@ def okreni_niz(broj):
     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':
+        if char == "1":
             okreni(i + 1)
 
 
 def main():
 
+    zidovi = ZidoviAction('/dev/ttyACM1')
+    zupcanik = ZupcanikAction('/dev/ttyACM1')
+
+
     # sekvenca 1
-      zupcanik(1, -1000, 25)
+    okreni(5)
+    zupcanik.zupcanik(1, -1010, 25)
 
     # sekvenca 2
-      beli_zid(1)
+    zidovi.beli_zid(1)
-      plavi_zid(1)
+    zidovi.plavi_zid(1)
 
     okreni_niz("1010")
 
-      plavi_zid(0)
+    zidovi.plavi_zid(0, TargetPos=150)
-      beli_zid(0)
+    zidovi.beli_zid(0, TargetPos=450)
+
 
     # sekvenca 3
-      zupcanik(1, 1000, 25)
+
+    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

@@ -1,17 +1,26 @@
-import sys
+from .STservo_sdk import PortHandler, sts, COMM_SUCCESS, STS_ID
-import os
-
-from .STservo_sdk import *
 # da li ovo dole treba da importujem ako je nalazi u STservo_sdk?
 # from stservo_def import COMM_SUCCESS
 
 # Default settings
-BAUDRATE = 1000000  # STServo default baudrate
-DEVICENAME = 'COM4'  # Check which port is being used on your controller
-STS_MOVING_ACC = 0  # STServo moving acceleration
 
 
-def plavi_zid(smer, brzina=1500, opterecenje_threshold=75,TargetPos=300): 
+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:
@@ -22,7 +31,7 @@ def plavi_zid(smer, brzina=1500, opterecenje_threshold=75,TargetPos=300):
             Speed_ID4 = brzina  # Speed for motor ID 4 (opposite sign)
 
         # Initialize PortHandler instance
-    portHandler = PortHandler(DEVICENAME)
+        portHandler = PortHandler(self.DEVICENAME)
 
         # Initialize PacketHandler instance
         packetHandler = sts(portHandler)
@@ -37,7 +46,7 @@ def plavi_zid(smer, brzina=1500, opterecenje_threshold=75,TargetPos=300):
             quit()
 
         # Set port baudrate
-    if portHandler.setBaudRate(BAUDRATE):
+        if portHandler.setBaudRate(self.BAUDRATE):
             print("Succeeded to change the baudrate")
         else:
             print("Failed to change the baudrate")
@@ -49,17 +58,33 @@ def plavi_zid(smer, brzina=1500, opterecenje_threshold=75,TargetPos=300):
         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)))
+                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)))
+                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_ID2, sts_present_speed_ID2, sts_comm_result, sts_error = (
-    sts_present_position_ID4, sts_present_speed_ID4, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(4)
+            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(
-    print("[ID:004] PresPos:%d PresSpd:%d" % (sts_present_position_ID4, sts_present_speed_ID4))
+            "[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
@@ -71,17 +96,19 @@ def plavi_zid(smer, brzina=1500, opterecenje_threshold=75,TargetPos=300):
         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, STS_MOVING_ACC)
+            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, STS_MOVING_ACC)
+            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:
@@ -91,13 +118,23 @@ def plavi_zid(smer, brzina=1500, opterecenje_threshold=75,TargetPos=300):
             ProslaPos_ID2 = TrenutnaPos_ID2
             ProslaPos_ID4 = TrenutnaPos_ID4
 
-        sts_present_position_ID2, sts_present_speed_ID2, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(2)
+            (
+                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)
+            (
+                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:
@@ -107,33 +144,47 @@ def plavi_zid(smer, brzina=1500, opterecenje_threshold=75,TargetPos=300):
             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(
-        print("[ID:004] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID4, sts_present_speed_ID4, opterecenje4))
+                "[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:
+            if (
-            print("High load detected: [ID:002] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID2, sts_present_speed_ID2, opterecenje2))
+                opterecenje2 > opterecenje_threshold
-            print("High load detected: [ID:004] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID4, sts_present_speed_ID4, opterecenje4))
+                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:
+            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.")
@@ -144,10 +195,7 @@ def plavi_zid(smer, brzina=1500, opterecenje_threshold=75,TargetPos=300):
         # Close port
         portHandler.closePort()
 
-
+    def beli_zid(self, smer, brzina=1500, opterecenje_threshold=75, TargetPos=600):
-    
-    
-def beli_zid(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:
@@ -158,9 +206,8 @@ def beli_zid(smer, brzina=1500, opterecenje_threshold=75, TargetPos=600):
             Speed_ID3 = brzina  # Speed for motor ID 3
             Speed_ID5 = -brzina  # Speed for motor ID 5 (opposite sign)
 
-
         # Initialize PortHandler instance
-    portHandler = PortHandler(DEVICENAME)
+        portHandler = PortHandler(self.DEVICENAME)
 
         # Initialize PacketHandler instance
         packetHandler = sts(portHandler)
@@ -175,7 +222,7 @@ def beli_zid(smer, brzina=1500, opterecenje_threshold=75, TargetPos=600):
             quit()
 
         # Set port baudrate
-    if portHandler.setBaudRate(BAUDRATE):
+        if portHandler.setBaudRate(self.BAUDRATE):
             print("Succeeded to change the baudrate")
         else:
             print("Failed to change the baudrate")
@@ -187,13 +234,23 @@ def beli_zid(smer, brzina=1500, opterecenje_threshold=75, TargetPos=600):
         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)))
+                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)))
+                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_ID3, sts_present_speed_ID3, sts_comm_result, sts_error = (
-    sts_present_position_ID5, sts_present_speed_ID5, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(5)
+            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))
@@ -209,16 +266,19 @@ def beli_zid(smer, brzina=1500, opterecenje_threshold=75, TargetPos=600):
         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, STS_MOVING_ACC)
+            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, STS_MOVING_ACC)
+            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:
@@ -228,13 +288,23 @@ def beli_zid(smer, brzina=1500, opterecenje_threshold=75, TargetPos=600):
             ProslaPos_ID3 = TrenutnaPos_ID3
             ProslaPos_ID5 = TrenutnaPos_ID5
 
-        sts_present_position_ID3, sts_present_speed_ID3, sts_comm_result, sts_error = packetHandler.ReadPosSpeed(3)
+            (
+                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)
+            (
+                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:
@@ -244,33 +314,47 @@ def beli_zid(smer, brzina=1500, opterecenje_threshold=75, TargetPos=600):
             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(
-        print("[ID:005] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5))
+                "[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:
+            if (
-            print("High load detected: [ID:003] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID3, sts_present_speed_ID3, opterecenje3))
+                opterecenje3 > opterecenje_threshold
-            print("High load detected: [ID:005] PresPos:%d PresSpd:%d Load:%d" % (sts_present_position_ID5, sts_present_speed_ID5, opterecenje5))
+                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:
+            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.")
@@ -280,5 +364,3 @@ def beli_zid(smer, brzina=1500, opterecenje_threshold=75, TargetPos=600):
 
         # Close port
         portHandler.closePort()
-
-

toid_interaction/toid_interaction/mechanism/zuocanik.py

@@ -1,105 +0,0 @@
-
-import sys
-import os
-
-
-from .STservo_sdk import *  # Uses STServo SDK library
-
-# Default setting
-
-BAUDRATE = 1000000  # STServo default baudrate : 1000000
-DEVICENAME = 'COM4'  # Check which port is being used on your controller
-STS_MOVING_ACC = 50  # STServo moving acc
-
-
-
-
-# Function to move the STServo to the target position
-def zupcanik(STS_ID ,STS_MOVING_SPEED , TargetPos):
-
-    # Initialize PortHandler instance
-    portHandler = PortHandler(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(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, 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_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/package.xml

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

toid_msgs/CMakeLists.txt

@@ -15,6 +15,7 @@ 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"

toid_msgs/srv/SendString.srv

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