mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
245 lines
8.4 KiB
Python
Executable file
245 lines
8.4 KiB
Python
Executable file
#!/usr/bin/env python3
|
|
# -*- coding:utf-8 -*-
|
|
"""
|
|
teleop_keyboard.py
|
|
This module provides keyboard teleoperation for the MyCobot Pro450 using ROS1 topics.
|
|
|
|
Controls:
|
|
- Cartesian movement: w/s/a/d/z/x
|
|
- Euler rotation: u/j, i/k, o/l
|
|
- Step size adjustment: +/-
|
|
- Gripper control: g/h
|
|
- Preset poses: 1 (init), 2 (home), 3 (save current as home)
|
|
- Quit: q
|
|
|
|
Author: WangWeiJian
|
|
Date: 2025-09-08
|
|
"""
|
|
|
|
from __future__ import print_function
|
|
import sys
|
|
import time
|
|
import termios
|
|
import tty
|
|
import rospy
|
|
from mycobot_pro450_communication.msg import (
|
|
MycobotAngles,
|
|
MycobotCoords,
|
|
MycobotSetAngles,
|
|
MycobotSetCoords,
|
|
MycobotGripperStatus,
|
|
)
|
|
|
|
# Teleop help message
|
|
MSG = """\
|
|
Mycobot Pro450 Teleop Keyboard Controller (ROS1 - Topic Version)
|
|
---------------------------------------------------------
|
|
Movement (Cartesian):
|
|
w (x+)
|
|
a (y+) s (x-) d (y-)
|
|
z (z-) x (z+)
|
|
|
|
Rotation (Euler angles):
|
|
u (rx+) i (ry+) o (rz+)
|
|
j (rx-) k (ry-) l (rz-)
|
|
|
|
Movement Step:
|
|
+ : Increase movement step size
|
|
- : Decrease movement step size
|
|
|
|
Gripper:
|
|
g - open h - close
|
|
|
|
Other:
|
|
1 - Go to init pose
|
|
2 - Go to home pose
|
|
3 - Save current pose as home
|
|
q - Quit
|
|
"""
|
|
|
|
# Coordinate limits
|
|
COORD_LIMITS = {
|
|
'x': (-466, 466),
|
|
'y': (-466, 466),
|
|
'z': (-230, 614),
|
|
'rx': (-180, 180),
|
|
'ry': (-180, 180),
|
|
'rz': (-180, 180)
|
|
}
|
|
|
|
|
|
def vels(speed: float, turn: float) -> str:
|
|
"""Return formatted speed and change percent info."""
|
|
return f"currently:\tspeed: {speed}\tchange percent: {turn}"
|
|
|
|
|
|
class Raw:
|
|
"""Context manager for reading raw input from terminal."""
|
|
|
|
def __init__(self, stream):
|
|
self.stream = stream
|
|
self.fd = self.stream.fileno()
|
|
|
|
def __enter__(self):
|
|
self.original_stty = termios.tcgetattr(self.stream)
|
|
tty.setcbreak(self.stream)
|
|
|
|
def __exit__(self, type, value, traceback):
|
|
termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty)
|
|
|
|
|
|
class MycobotTeleopTopic:
|
|
"""Keyboard teleoperation node for MyCobot Pro450 using ROS topics."""
|
|
|
|
def __init__(self):
|
|
"""Initialize ROS node, parameters, publishers, and subscribers."""
|
|
rospy.init_node("teleop_keyboard_topic")
|
|
|
|
# Speed and step size
|
|
self.speed = rospy.get_param("~speed", 50)
|
|
self.change_percent = rospy.get_param("~change_percent", 5)
|
|
self.change_len = 250 * self.change_percent / 100.0
|
|
self.change_angle = 180 * self.change_percent / 100.0
|
|
|
|
# Current robot state
|
|
self.curr_coords = [0] * 6
|
|
self.curr_angles = [0] * 6
|
|
self.record_coords = None
|
|
self.home_pose = [0, -10, -123, 45, 0, 0]
|
|
|
|
# Subscribers
|
|
rospy.Subscriber("mycobot/coords_real", MycobotCoords, self.coords_callback)
|
|
rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.angles_callback)
|
|
|
|
# Publishers
|
|
self.coords_pub = rospy.Publisher("mycobot/coords_goal", MycobotSetCoords, queue_size=1)
|
|
self.angles_pub = rospy.Publisher("mycobot/angles_goal", MycobotSetAngles, queue_size=1)
|
|
self.gripper_pub = rospy.Publisher("mycobot/gripper_status", MycobotGripperStatus, queue_size=1)
|
|
|
|
rospy.loginfo("Waiting to receive current coordinates...")
|
|
while self.curr_coords == [0] * 6 and not rospy.is_shutdown():
|
|
time.sleep(0.1)
|
|
|
|
self.record_coords = list(self.curr_coords)
|
|
print(MSG)
|
|
print(vels(self.speed, self.change_percent))
|
|
rospy.loginfo(
|
|
"Current moving step: position %.1f mm, angle attitude %.1f°",
|
|
self.change_len,
|
|
self.change_angle
|
|
)
|
|
|
|
def coords_callback(self, msg: MycobotCoords):
|
|
"""Update current coordinates from ROS topic."""
|
|
self.curr_coords = [msg.x, msg.y, msg.z, msg.rx, msg.ry, msg.rz]
|
|
|
|
def angles_callback(self, msg: MycobotAngles):
|
|
"""Update current angles from ROS topic."""
|
|
self.curr_angles = [
|
|
msg.joint_1, msg.joint_2, msg.joint_3,
|
|
msg.joint_4, msg.joint_5, msg.joint_6
|
|
]
|
|
|
|
def send_coords(self):
|
|
"""Publish current target coordinates to robot."""
|
|
goal = MycobotSetCoords()
|
|
goal.x, goal.y, goal.z, goal.rx, goal.ry, goal.rz = self.record_coords
|
|
goal.speed = self.speed
|
|
self.coords_pub.publish(goal)
|
|
|
|
def send_angles(self, angles):
|
|
"""Publish target joint angles to robot."""
|
|
goal = MycobotSetAngles()
|
|
goal.joint_1, goal.joint_2, goal.joint_3, goal.joint_4, goal.joint_5, goal.joint_6 = angles
|
|
goal.speed = self.speed
|
|
self.angles_pub.publish(goal)
|
|
|
|
def run(self):
|
|
"""Main loop to read keyboard input and control the robot."""
|
|
while not rospy.is_shutdown():
|
|
try:
|
|
with Raw(sys.stdin):
|
|
key = sys.stdin.read(1)
|
|
except Exception:
|
|
continue
|
|
|
|
try:
|
|
# Quit
|
|
if key == 'q':
|
|
break
|
|
|
|
# Adjust step size
|
|
elif key == '+':
|
|
self.change_percent = min(self.change_percent + 1, 20)
|
|
self.change_angle = 180 * self.change_percent / 100.0
|
|
self.change_len = 250 * self.change_percent / 100.0
|
|
rospy.loginfo(
|
|
"Increase change_percent to %d%%, move step: %.1f mm, angle step: %.1f°",
|
|
self.change_percent, self.change_len, self.change_angle
|
|
)
|
|
elif key == '-':
|
|
self.change_percent = max(self.change_percent - 1, 1)
|
|
self.change_angle = 180 * self.change_percent / 100.0
|
|
self.change_len = 250 * self.change_percent / 100.0
|
|
rospy.loginfo(
|
|
"Decrease change_percent to %d%%, move step: %.1f mm, angle step: %.1f°",
|
|
self.change_percent, self.change_len, self.change_angle
|
|
)
|
|
continue
|
|
|
|
# Cartesian movement
|
|
elif key in 'wW': self.record_coords[0] += self.change_len
|
|
elif key in 'sS': self.record_coords[0] -= self.change_len
|
|
elif key in 'aA': self.record_coords[1] += self.change_len
|
|
elif key in 'dD': self.record_coords[1] -= self.change_len
|
|
elif key in 'zZ': self.record_coords[2] -= self.change_len
|
|
elif key in 'xX': self.record_coords[2] += self.change_len
|
|
|
|
# Euler rotation
|
|
elif key in 'uU': self.record_coords[3] += self.change_angle
|
|
elif key in 'jJ': self.record_coords[3] -= self.change_angle
|
|
elif key in 'iI': self.record_coords[4] += self.change_angle
|
|
elif key in 'kK': self.record_coords[4] -= self.change_angle
|
|
elif key in 'oO': self.record_coords[5] += self.change_angle
|
|
elif key in 'lL': self.record_coords[5] -= self.change_angle
|
|
|
|
# Gripper control
|
|
elif key in 'gG':
|
|
self.gripper_pub.publish(MycobotGripperStatus(Status=True))
|
|
elif key in 'hH':
|
|
self.gripper_pub.publish(MycobotGripperStatus(Status=False))
|
|
|
|
# Preset poses
|
|
elif key == '1':
|
|
self.send_angles([0, 0, 0, 0, 0, 0])
|
|
time.sleep(2)
|
|
self.record_coords = list(self.curr_coords)
|
|
elif key == '2':
|
|
self.send_angles(self.home_pose)
|
|
time.sleep(2)
|
|
self.record_coords = list(self.curr_coords)
|
|
elif key == '3':
|
|
self.home_pose = list(self.curr_angles)
|
|
else:
|
|
continue
|
|
|
|
# Coordinate limits check
|
|
for val, axis in zip(self.record_coords, ['x', 'y', 'z', 'rx', 'ry', 'rz']):
|
|
min_v, max_v = COORD_LIMITS[axis]
|
|
if not (min_v <= val <= max_v):
|
|
rospy.logwarn(f"{axis} Out of range: {val} not in [{min_v}, {max_v}]")
|
|
raise ValueError("Out of range of motion")
|
|
|
|
self.send_coords()
|
|
|
|
except Exception as e:
|
|
rospy.logwarn("Execution failed: {}".format(e))
|
|
continue
|
|
|
|
|
|
if __name__ == '__main__':
|
|
try:
|
|
teleop = MycobotTeleopTopic()
|
|
teleop.run()
|
|
except rospy.ROSInterruptException:
|
|
pass
|