feat(module3): add simple competition workflow

module3/simple_gripper.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+from datetime import datetime
+from pathlib import Path
+import time
+
+import rclpy
+from builtin_interfaces.msg import Duration
+from control_msgs.action import GripperCommand
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+ACTION = "/RMC1/arm95/gripper_controller/gripper_cmd"
+TOPIC = "/RMC1/arm95/gripper_trajectory_controller/joint_trajectory"
+HERE = Path(__file__).resolve().parent
+
+
+class Gripper(Node):
+    def __init__(self):
+        super().__init__("simple_gripper")
+        self.client = ActionClient(self, GripperCommand, ACTION)
+        self.publisher = self.create_publisher(JointTrajectory, TOPIC, 10)
+
+    def set_state(self, opened):
+        state = "open" if opened else "closed"
+        if self.client.wait_for_server(timeout_sec=2.0):
+            goal = GripperCommand.Goal()
+            goal.command.position = 0.04 if opened else 0.0
+            goal.command.max_effort = 20.0
+            future = self.client.send_goal_async(goal)
+            rclpy.spin_until_future_complete(self, future)
+            handle = future.result()
+            if not handle.accepted:
+                raise RuntimeError("Контроллер отклонил команду")
+            result = handle.get_result_async()
+            rclpy.spin_until_future_complete(self, result)
+        else:
+            message = JointTrajectory()
+            message.joint_names = ["left_joint", "right_joint"]
+            point = JointTrajectoryPoint()
+            value = 0.04 if opened else 0.0213
+            point.positions = [value, value]
+            point.time_from_start = Duration(sec=1)
+            message.points = [point]
+            for _ in range(5):
+                self.publisher.publish(message)
+                rclpy.spin_once(self, timeout_sec=0.1)
+
+        line = f"{datetime.now().isoformat(timespec='seconds')} | gripper={state}"
+        print(line)
+        with (HERE / "logs/simple_gripper.log").open("a", encoding="utf-8") as log:
+            log.write(line + "\n")
+        time.sleep(1)
+
+
+def main():
+    rclpy.init()
+    node = Gripper()
+    try:
+        node.set_state(True)
+        input("Захват открыт. Enter — закрыть: ")
+        node.set_state(False)
+        input("Захват закрыт. Enter — снова открыть: ")
+        node.set_state(True)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()