73 lines
2.5 KiB
Python
73 lines
2.5 KiB
Python
#!/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()
|