mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-14 09:00:53 +00:00
update mercury_b1
This commit is contained in:
wangWking
parent
aed4c6feac
commit
2b690b8bcb
9 changed files with 32 additions and 895 deletions
|
|
@ -23,13 +23,13 @@ include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
|
||||||
catkin_install_python(PROGRAMS
|
catkin_install_python(PROGRAMS
|
||||||
scripts/follow_display.py
|
scripts/follow_display.py
|
||||||
scripts/slider_control.py
|
scripts/slider_control.py
|
||||||
scripts/teleop_keyboard.py
|
# scripts/teleop_keyboard.py
|
||||||
scripts/listen_real.py
|
# scripts/listen_real.py
|
||||||
scripts/listen_real_of_topic.py
|
# scripts/listen_real_of_topic.py
|
||||||
# scripts/detect_marker.py
|
# scripts/detect_marker.py
|
||||||
# scripts/following_marker.py
|
# scripts/following_marker.py
|
||||||
# scripts/follow_and_pump.py
|
# scripts/follow_and_pump.py
|
||||||
scripts/simple_gui.py
|
# scripts/simple_gui.py
|
||||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,23 +0,0 @@
|
||||||
<launch>
|
|
||||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
|
||||||
<arg name="port" default="/dev/ttyAMA1" />
|
|
||||||
<arg name="baud" default="115200" />
|
|
||||||
|
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/mercury_a1/mercury_a1.urdf"/>
|
|
||||||
<arg name="rvizconfig" default="$(find mercury_a1)/config/mercury_a1.rviz" />
|
|
||||||
<arg name="gui" default="false" />
|
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
|
||||||
|
|
||||||
<!-- Combinejoin values to TF ,将值合并到TF-->
|
|
||||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
|
||||||
<!-- Show in Rviz ，显示在Rviz-->
|
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
|
||||||
|
|
||||||
<include file="$(find mercury_a1_communication)/launch/communication_service.launch">
|
|
||||||
<arg name="port" value="$(arg port)" />
|
|
||||||
<arg name="baud" value="$(arg baud)" />
|
|
||||||
</include>
|
|
||||||
<node name="real_listener" pkg="mercury_a1" type="listen_real.py" />
|
|
||||||
<node name="simple_gui" pkg="mercury_a1" type="simple_gui.py" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,23 +0,0 @@
|
||||||
<launch>
|
|
||||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
|
||||||
<arg name="port" default="/dev/ttyAMA1" />
|
|
||||||
<arg name="baud" default="115200" />
|
|
||||||
|
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/mercury_a1/mercury_a1.urdf"/>
|
|
||||||
<arg name="rvizconfig" default="$(find mercury_a1)/config/mercury_a1.rviz" />
|
|
||||||
<arg name="gui" default="false" />
|
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
|
||||||
|
|
||||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
|
||||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
|
||||||
<!-- Show in Rviz ，显示在Rviz-->
|
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
|
||||||
|
|
||||||
<include file="$(find mercury_a1_communication)/launch/communication_service.launch">
|
|
||||||
<arg name="port" value="$(arg port)" />
|
|
||||||
<arg name="baud" value="$(arg baud)" />
|
|
||||||
</include>
|
|
||||||
<!-- listen and pub the real angles ，监听并发布真实角度-->
|
|
||||||
<node name="real_listener" pkg="mercury_a1" type="listen_real.py" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -13,17 +13,17 @@ def talker():
|
||||||
rospy.init_node("display", anonymous=True)
|
rospy.init_node("display", anonymous=True)
|
||||||
|
|
||||||
print("Try connect real Mercury...")
|
print("Try connect real Mercury...")
|
||||||
port1 = rospy.get_param("~port1", "/dev/ttyS0")
|
port1 = rospy.get_param("~port1", "/dev/ttyTHS1")
|
||||||
port2 = rospy.get_param("~port2", "/dev/ttyTHS1")
|
port2 = rospy.get_param("~port2", "/dev/ttyS0")
|
||||||
baud = rospy.get_param("~baud", 115200)
|
baud = rospy.get_param("~baud", 115200)
|
||||||
print("port1: {}, baud: {}\n".format(port1, baud))
|
print("port1: {}, baud: {}\n".format(port1, baud))
|
||||||
print("port2: {}, baud: {}\n".format(port2, baud))
|
print("port2: {}, baud: {}\n".format(port2, baud))
|
||||||
try:
|
try:
|
||||||
# left arm
|
# left arm
|
||||||
cx1 = Mercury(port1, baud)
|
l = Mercury(port1, baud)
|
||||||
time.sleep(0.02)
|
time.sleep(0.02)
|
||||||
# right arm
|
# right arm
|
||||||
cx2 = Mercury(port2, baud)
|
r = Mercury(port2, baud)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print(e)
|
print(e)
|
||||||
print(
|
print(
|
||||||
|
|
@ -34,9 +34,9 @@ def talker():
|
||||||
"""
|
"""
|
||||||
)
|
)
|
||||||
exit(1)
|
exit(1)
|
||||||
cx1.release_all_servos()
|
l.release_all_servos()
|
||||||
time.sleep(0.02)
|
time.sleep(0.02)
|
||||||
cx2.release_all_servos()
|
r.release_all_servos()
|
||||||
time.sleep(0.1)
|
time.sleep(0.1)
|
||||||
print("Rlease all servos over.\n")
|
print("Rlease all servos over.\n")
|
||||||
|
|
||||||
|
|
@ -79,11 +79,11 @@ def talker():
|
||||||
while not rospy.is_shutdown():
|
while not rospy.is_shutdown():
|
||||||
joint_state_send.header.stamp = rospy.Time.now()
|
joint_state_send.header.stamp = rospy.Time.now()
|
||||||
|
|
||||||
left_angles = cx1.get_angles()
|
left_angles = l.get_angles()
|
||||||
right_angles = cx2.get_angles()
|
right_angles = r.get_angles()
|
||||||
eye_angle = cx2.get_angle(11)
|
eye_angle = r.get_angle(11)
|
||||||
head_angle = cx2.get_angle(12)
|
head_angle = r.get_angle(12)
|
||||||
body_angle = cx2.get_angle(13)
|
body_angle = r.get_angle(13)
|
||||||
|
|
||||||
print('left:', left_angles)
|
print('left:', left_angles)
|
||||||
print('right:', right_angles)
|
print('right:', right_angles)
|
||||||
|
|
@ -102,15 +102,15 @@ def talker():
|
||||||
|
|
||||||
pub.publish(joint_state_send)
|
pub.publish(joint_state_send)
|
||||||
|
|
||||||
left_coords = cx1.get_coords()
|
left_coords = l.get_coords()
|
||||||
|
|
||||||
right_coords = cx2.get_coords()
|
right_coords = r.get_coords()
|
||||||
|
|
||||||
eye_coords = cx2.get_angle(11)
|
eye_coords = r.get_angle(11)
|
||||||
|
|
||||||
head_coords = cx2.get_angle(12)
|
head_coords = r.get_angle(12)
|
||||||
|
|
||||||
body_coords = cx2.get_angle(13)
|
body_coords = r.get_angle(13)
|
||||||
|
|
||||||
|
|
||||||
# marker
|
# marker
|
||||||
|
|
|
||||||
|
|
@ -1,68 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
# encoding:utf-8
|
|
||||||
# license removed for brevity
|
|
||||||
import time
|
|
||||||
import math
|
|
||||||
|
|
||||||
import rospy
|
|
||||||
from sensor_msgs.msg import JointState
|
|
||||||
from std_msgs.msg import Header
|
|
||||||
from mercury_a1_communication.srv import GetAngles
|
|
||||||
|
|
||||||
|
|
||||||
def talker():
|
|
||||||
rospy.loginfo("start ...")
|
|
||||||
rospy.init_node("real_listener", anonymous=True)
|
|
||||||
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
|
||||||
rate = rospy.Rate(30) # 30hz
|
|
||||||
|
|
||||||
# pub joint state，发布关节状态
|
|
||||||
joint_state_send = JointState()
|
|
||||||
joint_state_send.header = Header()
|
|
||||||
|
|
||||||
joint_state_send.name = [
|
|
||||||
"joint1_to_base",
|
|
||||||
"joint2_to_joint1",
|
|
||||||
"joint3_to_joint2",
|
|
||||||
"joint4_to_joint3",
|
|
||||||
"joint5_to_joint4",
|
|
||||||
"joint6_to_joint5",
|
|
||||||
"joint7_to_joint6",
|
|
||||||
]
|
|
||||||
joint_state_send.velocity = [0]
|
|
||||||
joint_state_send.effort = []
|
|
||||||
|
|
||||||
# waiting util server `get_joint_angles` enable.等待'get_joint_angles'服务启用
|
|
||||||
rospy.loginfo("wait service")
|
|
||||||
rospy.wait_for_service("get_joint_angles")
|
|
||||||
func = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
|
||||||
|
|
||||||
rospy.loginfo("start loop ...")
|
|
||||||
while not rospy.is_shutdown():
|
|
||||||
# get real angles from server.从服务器获得真实的角度。
|
|
||||||
res = func()
|
|
||||||
if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
|
|
||||||
continue
|
|
||||||
radians_list = [
|
|
||||||
res.joint_1 * (math.pi / 180),
|
|
||||||
res.joint_2 * (math.pi / 180),
|
|
||||||
res.joint_3 * (math.pi / 180),
|
|
||||||
res.joint_4 * (math.pi / 180),
|
|
||||||
res.joint_5 * (math.pi / 180),
|
|
||||||
res.joint_6 * (math.pi / 180),
|
|
||||||
res.joint_7 * (math.pi / 180),
|
|
||||||
]
|
|
||||||
rospy.loginfo("res: {}".format(radians_list))
|
|
||||||
|
|
||||||
# publish angles.发布角度
|
|
||||||
joint_state_send.header.stamp = rospy.Time.now()
|
|
||||||
joint_state_send.position = radians_list
|
|
||||||
pub.publish(joint_state_send)
|
|
||||||
rate.sleep()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
try:
|
|
||||||
talker()
|
|
||||||
except rospy.ROSInterruptException:
|
|
||||||
pass
|
|
||||||
|
|
@ -1,66 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
# encoding:utf-8
|
|
||||||
|
|
||||||
import math
|
|
||||||
import rospy
|
|
||||||
from sensor_msgs.msg import JointState
|
|
||||||
from std_msgs.msg import Header
|
|
||||||
from mercury_a1_communication.msg import CobotXAngles
|
|
||||||
|
|
||||||
|
|
||||||
class Listener(object):
|
|
||||||
def __init__(self):
|
|
||||||
super(Listener, self).__init__()
|
|
||||||
|
|
||||||
rospy.loginfo("start ...")
|
|
||||||
rospy.init_node("real_listener_1", anonymous=True)
|
|
||||||
# init publisher.初始化发布者
|
|
||||||
self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
|
||||||
# init subscriber.初始化订阅者
|
|
||||||
self.sub = rospy.Subscriber("mercury/angles_real", CobotXAngles, self.callback)
|
|
||||||
rospy.spin()
|
|
||||||
|
|
||||||
def callback(self, data):
|
|
||||||
"""`cobotx/angles_real` subscriber callback method.
|
|
||||||
|
|
||||||
Args:
|
|
||||||
data (CobotXAngles): callback argument.
|
|
||||||
"""
|
|
||||||
# ini publisher object. 初始化发布者对象
|
|
||||||
joint_state_send = JointState()
|
|
||||||
joint_state_send.header = Header()
|
|
||||||
|
|
||||||
joint_state_send.name = [
|
|
||||||
"joint1_to_base",
|
|
||||||
"joint2_to_joint1",
|
|
||||||
"joint3_to_joint2",
|
|
||||||
"joint4_to_joint3",
|
|
||||||
"joint5_to_joint4",
|
|
||||||
"joint6_to_joint5",
|
|
||||||
"joint7_to_joint6",
|
|
||||||
]
|
|
||||||
joint_state_send.velocity = [0]
|
|
||||||
joint_state_send.effort = []
|
|
||||||
joint_state_send.header.stamp = rospy.Time.now()
|
|
||||||
|
|
||||||
# process callback data. 处理回调数据。
|
|
||||||
radians_list = [
|
|
||||||
data.joint_1 * (math.pi / 180),
|
|
||||||
data.joint_2 * (math.pi / 180),
|
|
||||||
data.joint_3 * (math.pi / 180),
|
|
||||||
data.joint_4 * (math.pi / 180),
|
|
||||||
data.joint_5 * (math.pi / 180),
|
|
||||||
data.joint_6 * (math.pi / 180),
|
|
||||||
data.joint_7 * (math.pi / 180),
|
|
||||||
]
|
|
||||||
rospy.loginfo("res: {}".format(radians_list))
|
|
||||||
|
|
||||||
joint_state_send.position = radians_list
|
|
||||||
self.pub.publish(joint_state_send)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
try:
|
|
||||||
Listener()
|
|
||||||
except rospy.ROSInterruptException:
|
|
||||||
pass
|
|
||||||
|
|
@ -1,501 +0,0 @@
|
||||||
#!/usr/bin/env python
|
|
||||||
# -*- coding: utf-8 -*-
|
|
||||||
try:
|
|
||||||
import tkinter as tk
|
|
||||||
except ImportError:
|
|
||||||
import Tkinter as tk
|
|
||||||
from mercury_a1_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
|
||||||
import rospy
|
|
||||||
import time
|
|
||||||
from rospy import ServiceException
|
|
||||||
|
|
||||||
|
|
||||||
class Window:
|
|
||||||
def __init__(self, handle):
|
|
||||||
self.win = handle
|
|
||||||
self.win.resizable(0, 0) # fixed window size，固定窗口大小
|
|
||||||
|
|
||||||
self.model = 0
|
|
||||||
self.speed = rospy.get_param("~speed", 50)
|
|
||||||
|
|
||||||
# set default speed，设置默认速度
|
|
||||||
self.speed_d = tk.StringVar()
|
|
||||||
self.speed_d.set(str(self.speed))
|
|
||||||
# print(self.speed)
|
|
||||||
self.connect_ser()
|
|
||||||
|
|
||||||
# Get the data of the robotic arm，获取机械臂数据
|
|
||||||
self.record_coords = [0, 0, 0, 0, 0, 0, self.speed, self.model]
|
|
||||||
self.res_angles = [0, 0, 0, 0, 0, 0, self.speed, self.model]
|
|
||||||
self.get_date()
|
|
||||||
|
|
||||||
# get screen width and height.获取屏幕宽度和高度
|
|
||||||
self.ws = self.win.winfo_screenwidth() # width of the screen
|
|
||||||
self.hs = self.win.winfo_screenheight() # height of the screen
|
|
||||||
# calculate x and y coordinates for the Tk root window
|
|
||||||
# 计算 Tk 根窗口的 x 和 y 坐标
|
|
||||||
x = int((self.ws / 2) - 190)
|
|
||||||
y = int((self.hs / 2) - 250)
|
|
||||||
self.win.geometry("430x450+{}+{}".format(x, y))
|
|
||||||
# layout,布局
|
|
||||||
self.set_layout()
|
|
||||||
# input section,输入部分
|
|
||||||
self.need_input()
|
|
||||||
# Show part,展示部分
|
|
||||||
self.show_init()
|
|
||||||
|
|
||||||
# Set the joint buttons 设置joint按钮
|
|
||||||
tk.Button(self.frmLT, text="设置", width=5, command=self.get_joint_input).grid(
|
|
||||||
row=7, column=1, sticky="w", padx=3, pady=2
|
|
||||||
)
|
|
||||||
|
|
||||||
# coordination settings button,coordination 设置按钮
|
|
||||||
tk.Button(self.frmRT, text="设置", width=5, command=self.get_coord_input).grid(
|
|
||||||
row=7, column=1, sticky="w", padx=3, pady=2
|
|
||||||
)
|
|
||||||
|
|
||||||
# Gripper switch button,夹爪开关按钮
|
|
||||||
tk.Button(self.frmLB, text="夹爪(开)", command=self.gripper_open, width=5).grid(
|
|
||||||
row=1, column=0, sticky="w", padx=3, pady=50
|
|
||||||
)
|
|
||||||
tk.Button(self.frmLB, text="夹爪(关)", command=self.gripper_close, width=5).grid(
|
|
||||||
row=1, column=1, sticky="w", padx=3, pady=2
|
|
||||||
)
|
|
||||||
|
|
||||||
def connect_ser(self):
|
|
||||||
rospy.init_node("simple_gui", anonymous=True, disable_signals=True)
|
|
||||||
|
|
||||||
rospy.wait_for_service("get_joint_angles")
|
|
||||||
rospy.wait_for_service("set_joint_angles")
|
|
||||||
rospy.wait_for_service("get_joint_coords")
|
|
||||||
rospy.wait_for_service("set_joint_coords")
|
|
||||||
rospy.wait_for_service("switch_gripper_status")
|
|
||||||
try:
|
|
||||||
self.get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
|
|
||||||
self.set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
|
|
||||||
self.get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
|
||||||
self.set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
|
|
||||||
self.switch_gripper = rospy.ServiceProxy(
|
|
||||||
"switch_gripper_status", GripperStatus
|
|
||||||
)
|
|
||||||
except:
|
|
||||||
print("start error ...")
|
|
||||||
exit(1)
|
|
||||||
|
|
||||||
print("Connect service success.")
|
|
||||||
|
|
||||||
def set_layout(self):
|
|
||||||
self.frmLT = tk.Frame(width=200, height=200)
|
|
||||||
self.frmLC = tk.Frame(width=200, height=200)
|
|
||||||
self.frmLB = tk.Frame(width=200, height=200)
|
|
||||||
self.frmRT = tk.Frame(width=200, height=200)
|
|
||||||
self.frmLT.grid(row=0, column=0, padx=1, pady=3)
|
|
||||||
self.frmLC.grid(row=1, column=0, padx=1, pady=3)
|
|
||||||
self.frmLB.grid(row=1, column=1, padx=2, pady=3)
|
|
||||||
self.frmRT.grid(row=0, column=1, padx=2, pady=3)
|
|
||||||
|
|
||||||
def need_input(self):
|
|
||||||
# input hint,输入提示
|
|
||||||
tk.Label(self.frmLT, text="Joint 1 ").grid(row=0)
|
|
||||||
tk.Label(self.frmLT, text="Joint 2 ").grid(row=1) # the second row,第二行
|
|
||||||
tk.Label(self.frmLT, text="Joint 3 ").grid(row=2)
|
|
||||||
tk.Label(self.frmLT, text="Joint 4 ").grid(row=3)
|
|
||||||
tk.Label(self.frmLT, text="Joint 5 ").grid(row=4)
|
|
||||||
tk.Label(self.frmLT, text="Joint 6 ").grid(row=5)
|
|
||||||
tk.Label(self.frmLT, text="Joint 7 ").grid(row=6)
|
|
||||||
|
|
||||||
tk.Label(self.frmRT, text=" x ").grid(row=0)
|
|
||||||
tk.Label(self.frmRT, text=" y ").grid(row=1) # the second row,第二行
|
|
||||||
tk.Label(self.frmRT, text=" z ").grid(row=2)
|
|
||||||
tk.Label(self.frmRT, text=" rx ").grid(row=3)
|
|
||||||
tk.Label(self.frmRT, text=" ry ").grid(row=4)
|
|
||||||
tk.Label(self.frmRT, text=" rz ").grid(row=5)
|
|
||||||
|
|
||||||
# Set the default value of the input box,设置输入框的默认值
|
|
||||||
self.j1_default = tk.StringVar()
|
|
||||||
self.j1_default.set(self.res_angles[0])
|
|
||||||
self.j2_default = tk.StringVar()
|
|
||||||
self.j2_default.set(self.res_angles[1])
|
|
||||||
self.j3_default = tk.StringVar()
|
|
||||||
self.j3_default.set(self.res_angles[2])
|
|
||||||
self.j4_default = tk.StringVar()
|
|
||||||
self.j4_default.set(self.res_angles[3])
|
|
||||||
self.j5_default = tk.StringVar()
|
|
||||||
self.j5_default.set(self.res_angles[4])
|
|
||||||
self.j6_default = tk.StringVar()
|
|
||||||
self.j6_default.set(self.res_angles[5])
|
|
||||||
self.j7_default = tk.StringVar()
|
|
||||||
self.j7_default.set(self.res_angles[6])
|
|
||||||
|
|
||||||
|
|
||||||
self.x_default = tk.StringVar()
|
|
||||||
self.x_default.set(self.record_coords[0])
|
|
||||||
self.y_default = tk.StringVar()
|
|
||||||
self.y_default.set(self.record_coords[1])
|
|
||||||
self.z_default = tk.StringVar()
|
|
||||||
self.z_default.set(self.record_coords[2])
|
|
||||||
self.rx_default = tk.StringVar()
|
|
||||||
self.rx_default.set(self.record_coords[3])
|
|
||||||
self.ry_default = tk.StringVar()
|
|
||||||
self.ry_default.set(self.record_coords[4])
|
|
||||||
self.rz_default = tk.StringVar()
|
|
||||||
self.rz_default.set(self.record_coords[5])
|
|
||||||
|
|
||||||
# joint input box,joint 输入框
|
|
||||||
self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default)
|
|
||||||
self.J_1.grid(row=0, column=1, pady=3)
|
|
||||||
self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default)
|
|
||||||
self.J_2.grid(row=1, column=1, pady=3)
|
|
||||||
self.J_3 = tk.Entry(self.frmLT, textvariable=self.j3_default)
|
|
||||||
self.J_3.grid(row=2, column=1, pady=3)
|
|
||||||
self.J_4 = tk.Entry(self.frmLT, textvariable=self.j4_default)
|
|
||||||
self.J_4.grid(row=3, column=1, pady=3)
|
|
||||||
self.J_5 = tk.Entry(self.frmLT, textvariable=self.j5_default)
|
|
||||||
self.J_5.grid(row=4, column=1, pady=3)
|
|
||||||
self.J_6 = tk.Entry(self.frmLT, textvariable=self.j6_default)
|
|
||||||
self.J_6.grid(row=5, column=1, pady=3)
|
|
||||||
self.J_7 = tk.Entry(self.frmLT, textvariable=self.j7_default)
|
|
||||||
self.J_7.grid(row=6, column=1, pady=3)
|
|
||||||
|
|
||||||
# coord input box,coord 输入框
|
|
||||||
self.x = tk.Entry(self.frmRT, textvariable=self.x_default)
|
|
||||||
self.x.grid(row=0, column=1, pady=3, padx=0)
|
|
||||||
self.y = tk.Entry(self.frmRT, textvariable=self.y_default)
|
|
||||||
self.y.grid(row=1, column=1, pady=3)
|
|
||||||
self.z = tk.Entry(self.frmRT, textvariable=self.z_default)
|
|
||||||
self.z.grid(row=2, column=1, pady=3)
|
|
||||||
self.rx = tk.Entry(self.frmRT, textvariable=self.rx_default)
|
|
||||||
self.rx.grid(row=3, column=1, pady=3)
|
|
||||||
self.ry = tk.Entry(self.frmRT, textvariable=self.ry_default)
|
|
||||||
self.ry.grid(row=4, column=1, pady=3)
|
|
||||||
self.rz = tk.Entry(self.frmRT, textvariable=self.rz_default)
|
|
||||||
self.rz.grid(row=5, column=1, pady=3)
|
|
||||||
|
|
||||||
# All input boxes, used to get the input data,所有输入框，用于拿输入的数据
|
|
||||||
self.all_j = [self.J_1, self.J_2, self.J_3, self.J_4, self.J_5, self.J_6, self.J_7]
|
|
||||||
self.all_c = [self.x, self.y, self.z, self.rx, self.ry, self.rz]
|
|
||||||
|
|
||||||
# speed input box,速度输入框
|
|
||||||
tk.Label(
|
|
||||||
self.frmLB,
|
|
||||||
text="speed",
|
|
||||||
).grid(row=0, column=0)
|
|
||||||
self.get_speed = tk.Entry(self.frmLB, textvariable=self.speed_d, width=10)
|
|
||||||
self.get_speed.grid(row=0, column=1)
|
|
||||||
|
|
||||||
def show_init(self):
|
|
||||||
# show,显示
|
|
||||||
tk.Label(self.frmLC, text="Joint 1 ").grid(row=0)
|
|
||||||
tk.Label(self.frmLC, text="Joint 2 ").grid(row=1) # the second row,第二行
|
|
||||||
tk.Label(self.frmLC, text="Joint 3 ").grid(row=2)
|
|
||||||
tk.Label(self.frmLC, text="Joint 4 ").grid(row=3)
|
|
||||||
tk.Label(self.frmLC, text="Joint 5 ").grid(row=4)
|
|
||||||
tk.Label(self.frmLC, text="Joint 6 ").grid(row=5)
|
|
||||||
tk.Label(self.frmLC, text="Joint 7 ").grid(row=6)
|
|
||||||
|
|
||||||
# get数据
|
|
||||||
|
|
||||||
# show,展示出来
|
|
||||||
self.cont_1 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_1.set(str(self.res_angles[0]) + "°")
|
|
||||||
self.cont_2 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_2.set(str(self.res_angles[1]) + "°")
|
|
||||||
self.cont_3 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_3.set(str(self.res_angles[2]) + "°")
|
|
||||||
self.cont_4 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_4.set(str(self.res_angles[3]) + "°")
|
|
||||||
self.cont_5 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_5.set(str(self.res_angles[4]) + "°")
|
|
||||||
self.cont_6 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_6.set(str(self.res_angles[5]) + "°")
|
|
||||||
self.cont_7 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_7.set(str(self.res_angles[6]) + "°")
|
|
||||||
self.cont_all = [
|
|
||||||
self.cont_1,
|
|
||||||
self.cont_2,
|
|
||||||
self.cont_3,
|
|
||||||
self.cont_4,
|
|
||||||
self.cont_5,
|
|
||||||
self.cont_6,
|
|
||||||
self.cont_7,
|
|
||||||
self.speed,
|
|
||||||
self.model,
|
|
||||||
]
|
|
||||||
|
|
||||||
self.show_j1 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_1,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=0, column=1, padx=0, pady=5)
|
|
||||||
|
|
||||||
self.show_j2 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_2,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=1, column=1, padx=0, pady=5)
|
|
||||||
self.show_j3 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_3,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=2, column=1, padx=0, pady=5)
|
|
||||||
self.show_j4 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_4,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=3, column=1, padx=0, pady=5)
|
|
||||||
self.show_j5 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_5,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=4, column=1, padx=0, pady=5)
|
|
||||||
self.show_j6 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_6,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=5, column=1, padx=5, pady=5)
|
|
||||||
self.show_j7 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_7,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=6, column=1, padx=5, pady=5)
|
|
||||||
|
|
||||||
self.all_jo = [
|
|
||||||
self.show_j1,
|
|
||||||
self.show_j2,
|
|
||||||
self.show_j3,
|
|
||||||
self.show_j4,
|
|
||||||
self.show_j5,
|
|
||||||
self.show_j6,
|
|
||||||
self.show_j7,
|
|
||||||
]
|
|
||||||
|
|
||||||
# show,显示
|
|
||||||
tk.Label(self.frmLC, text=" x ").grid(row=0, column=3)
|
|
||||||
tk.Label(self.frmLC, text=" y ").grid(row=1, column=3)
|
|
||||||
tk.Label(self.frmLC, text=" z ").grid(row=2, column=3)
|
|
||||||
tk.Label(self.frmLC, text=" rx ").grid(row=3, column=3)
|
|
||||||
tk.Label(self.frmLC, text=" ry ").grid(row=4, column=3)
|
|
||||||
tk.Label(self.frmLC, text=" rz ").grid(row=5, column=3)
|
|
||||||
self.coord_x = tk.StringVar()
|
|
||||||
self.coord_x.set(str(self.record_coords[0]))
|
|
||||||
self.coord_y = tk.StringVar()
|
|
||||||
self.coord_y.set(str(self.record_coords[1]))
|
|
||||||
self.coord_z = tk.StringVar()
|
|
||||||
self.coord_z.set(str(self.record_coords[2]))
|
|
||||||
self.coord_rx = tk.StringVar()
|
|
||||||
self.coord_rx.set(str(self.record_coords[3]))
|
|
||||||
self.coord_ry = tk.StringVar()
|
|
||||||
self.coord_ry.set(str(self.record_coords[4]))
|
|
||||||
self.coord_rz = tk.StringVar()
|
|
||||||
self.coord_rz.set(str(self.record_coords[5]))
|
|
||||||
|
|
||||||
self.coord_all = [
|
|
||||||
self.coord_x,
|
|
||||||
self.coord_y,
|
|
||||||
self.coord_z,
|
|
||||||
self.coord_rx,
|
|
||||||
self.coord_ry,
|
|
||||||
self.coord_rz,
|
|
||||||
self.speed,
|
|
||||||
self.model,
|
|
||||||
]
|
|
||||||
|
|
||||||
self.show_x = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.coord_x,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=0, column=4, padx=5, pady=5)
|
|
||||||
self.show_y = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.coord_y,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=1, column=4, padx=5, pady=5)
|
|
||||||
self.show_z = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.coord_z,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=2, column=4, padx=5, pady=5)
|
|
||||||
self.show_rx = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.coord_rx,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=3, column=4, padx=5, pady=5)
|
|
||||||
self.show_ry = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.coord_ry,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=4, column=4, padx=5, pady=5)
|
|
||||||
self.show_rz = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.coord_rz,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=5, column=4, padx=5, pady=5)
|
|
||||||
|
|
||||||
# mm， Unit show，单位展示
|
|
||||||
self.unit = tk.StringVar()
|
|
||||||
self.unit.set("mm")
|
|
||||||
for i in range(6):
|
|
||||||
tk.Label(self.frmLC, textvariable=self.unit, font=("Arial", 9)).grid(
|
|
||||||
row=i, column=5
|
|
||||||
)
|
|
||||||
|
|
||||||
def gripper_open(self):
|
|
||||||
try:
|
|
||||||
self.switch_gripper(True)
|
|
||||||
except ServiceException:
|
|
||||||
# Probably because the method has no return value, the service throws an unhandled error
|
|
||||||
# 可能由于该方法没有返回值，服务抛出无法处理的错误
|
|
||||||
pass
|
|
||||||
|
|
||||||
def gripper_close(self):
|
|
||||||
try:
|
|
||||||
self.switch_gripper(False)
|
|
||||||
except ServiceException:
|
|
||||||
pass
|
|
||||||
|
|
||||||
def get_coord_input(self):
|
|
||||||
# Get the data input by coord and send it to the robotic arm
|
|
||||||
# 获取 coord 输入的数据，发送给机械臂
|
|
||||||
c_value = []
|
|
||||||
for i in self.all_c:
|
|
||||||
# print(type(i.get()))
|
|
||||||
c_value.append(float(i.get()))
|
|
||||||
self.speed = (
|
|
||||||
int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
|
|
||||||
)
|
|
||||||
c_value.append(self.speed)
|
|
||||||
c_value.append(self.model)
|
|
||||||
# print(c_value)
|
|
||||||
try:
|
|
||||||
self.set_coords(*c_value)
|
|
||||||
except ServiceException:
|
|
||||||
pass
|
|
||||||
self.show_j_date(c_value[:-2], "coord")
|
|
||||||
|
|
||||||
def get_joint_input(self):
|
|
||||||
# Get the data input by the joint and send it to the robotic arm
|
|
||||||
# 获取joint输入的数据，发送给机械臂
|
|
||||||
j_value = []
|
|
||||||
for i in self.all_j:
|
|
||||||
# print(type(i.get()))
|
|
||||||
j_value.append(float(i.get()))
|
|
||||||
self.speed = (
|
|
||||||
int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
|
|
||||||
)
|
|
||||||
j_value.append(self.speed)
|
|
||||||
|
|
||||||
try:
|
|
||||||
self.set_angles(*j_value)
|
|
||||||
except ServiceException:
|
|
||||||
pass
|
|
||||||
self.show_j_date(j_value[:-1])
|
|
||||||
# return j_value,c_value,speed
|
|
||||||
|
|
||||||
def get_date(self):
|
|
||||||
# Take the data of the robotic arm for display.拿机械臂的数据，用于展示
|
|
||||||
t = time.time()
|
|
||||||
while time.time() - t < 2:
|
|
||||||
self.res = self.get_coords()
|
|
||||||
if self.res.x > 1:
|
|
||||||
break
|
|
||||||
time.sleep(0.1)
|
|
||||||
|
|
||||||
t = time.time()
|
|
||||||
while time.time() - t < 2:
|
|
||||||
self.angles = self.get_angles()
|
|
||||||
if self.angles.joint_1 > 1:
|
|
||||||
break
|
|
||||||
time.sleep(0.1)
|
|
||||||
# print(self.angles.joint_1)
|
|
||||||
self.record_coords = [
|
|
||||||
round(self.res.x, 2),
|
|
||||||
round(self.res.y, 2),
|
|
||||||
round(self.res.z, 2),
|
|
||||||
round(self.res.rx, 2),
|
|
||||||
round(self.res.ry, 2),
|
|
||||||
round(self.res.rz, 2),
|
|
||||||
self.speed,
|
|
||||||
self.model,
|
|
||||||
]
|
|
||||||
self.res_angles = [
|
|
||||||
round(self.angles.joint_1, 2),
|
|
||||||
round(self.angles.joint_2, 2),
|
|
||||||
round(self.angles.joint_3, 2),
|
|
||||||
round(self.angles.joint_4, 2),
|
|
||||||
round(self.angles.joint_5, 2),
|
|
||||||
round(self.angles.joint_6, 2),
|
|
||||||
round(self.angles.joint_7, 2),
|
|
||||||
]
|
|
||||||
# print('coord:',self.record_coords)
|
|
||||||
# print('angles:',self.res_angles)
|
|
||||||
|
|
||||||
# def send_input(self,dates):
|
|
||||||
def show_j_date(self, date, way=""):
|
|
||||||
# Show data,展示数据
|
|
||||||
if way == "coord":
|
|
||||||
for i, j in zip(date, self.coord_all):
|
|
||||||
# print(i)
|
|
||||||
j.set(str(i))
|
|
||||||
else:
|
|
||||||
for i, j in zip(date, self.cont_all):
|
|
||||||
j.set(str(i) + "°")
|
|
||||||
|
|
||||||
def run(self):
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
self.win.update()
|
|
||||||
time.sleep(0.001)
|
|
||||||
except tk.TclError as e:
|
|
||||||
if "application has been destroyed" in str(e):
|
|
||||||
break
|
|
||||||
else:
|
|
||||||
raise
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
window = tk.Tk()
|
|
||||||
window.title("CobotX ros GUI")
|
|
||||||
Window(window).run()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
|
|
@ -16,10 +16,10 @@ from sensor_msgs.msg import JointState
|
||||||
from pymycobot.mercury import Mercury
|
from pymycobot.mercury import Mercury
|
||||||
|
|
||||||
# left arm port
|
# left arm port
|
||||||
cx1 = None
|
l = None
|
||||||
|
|
||||||
# right arm port
|
# right arm port
|
||||||
cx2 = None
|
r = None
|
||||||
|
|
||||||
|
|
||||||
def callback(data):
|
def callback(data):
|
||||||
|
|
@ -39,33 +39,30 @@ def callback(data):
|
||||||
print('right_arm:', right_arm)
|
print('right_arm:', right_arm)
|
||||||
print('middle_arm:', middle_arm)
|
print('middle_arm:', middle_arm)
|
||||||
|
|
||||||
cx1.send_angles(left_arm, 50)
|
l.send_angles(left_arm, 50)
|
||||||
time.sleep(0.02)
|
time.sleep(0.02)
|
||||||
cx2.send_angles(right_arm, 50)
|
r.send_angles(right_arm, 50)
|
||||||
time.sleep(0.02)
|
time.sleep(0.02)
|
||||||
cx2.send_angle(11, middle_arm[0], 50)
|
r.send_angle(11, middle_arm[0], 50)
|
||||||
time.sleep(0.02)
|
time.sleep(0.02)
|
||||||
cx2.send_angle(12, middle_arm[1], 50)
|
r.send_angle(12, middle_arm[1], 50)
|
||||||
time.sleep(0.02)
|
time.sleep(0.02)
|
||||||
cx2.send_angle(13, middle_arm[2], 50)
|
r.send_angle(13, middle_arm[2], 50)
|
||||||
time.sleep(0.02)
|
time.sleep(0.02)
|
||||||
# mc.send_radians(data_list, 80)
|
|
||||||
# mc.send_angles(data_list, 80)
|
|
||||||
# time.sleep(0.5)
|
|
||||||
|
|
||||||
|
|
||||||
def listener():
|
def listener():
|
||||||
global cx1, cx2
|
global l, r
|
||||||
rospy.init_node("control_slider", anonymous=True)
|
rospy.init_node("control_slider", anonymous=True)
|
||||||
|
|
||||||
rospy.Subscriber("joint_states", JointState, callback)
|
rospy.Subscriber("joint_states", JointState, callback)
|
||||||
port1 = rospy.get_param("~port1", "/dev/ttyS0")
|
port1 = rospy.get_param("~port1", "/dev/ttyTHS1")
|
||||||
port2 = rospy.get_param("~port2", "/dev/ttyTHS1")
|
port2 = rospy.get_param("~port2", "/dev/ttyS0")
|
||||||
baud = rospy.get_param("~baud", 115200)
|
baud = rospy.get_param("~baud", 115200)
|
||||||
print(port1, baud)
|
print(port1, baud)
|
||||||
print(port2, baud)
|
print(port2, baud)
|
||||||
cx1 = Mercury(port1, baud)
|
l = Mercury(port1, baud)
|
||||||
cx2 = Mercury(port2, baud)
|
r = Mercury(port2, baud)
|
||||||
|
|
||||||
# spin() simply keeps python from exiting until this node is stopped
|
# spin() simply keeps python from exiting until this node is stopped
|
||||||
# spin()只是阻止python退出，直到该节点停止
|
# spin()只是阻止python退出，直到该节点停止
|
||||||
|
|
|
||||||
|
|
@ -1,179 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
from __future__ import print_function
|
|
||||||
from mercury_a1_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
|
||||||
import rospy
|
|
||||||
import sys
|
|
||||||
import select
|
|
||||||
import termios
|
|
||||||
import tty
|
|
||||||
import time
|
|
||||||
|
|
||||||
import roslib
|
|
||||||
|
|
||||||
# Terminal output prompt information. 终端输出提示信息
|
|
||||||
msg = """\
|
|
||||||
CobotX Teleop Keyboard Controller
|
|
||||||
---------------------------
|
|
||||||
Movimg options(control coordinations [x,y,z,rx,ry,rz]):
|
|
||||||
w(x+)
|
|
||||||
|
|
||||||
a(y-) s(x-) d(y+)
|
|
||||||
|
|
||||||
z(z-) x(z+)
|
|
||||||
|
|
||||||
u(rx+) i(ry+) o(rz+)
|
|
||||||
j(rx-) k(ry-) l(rz-)
|
|
||||||
|
|
||||||
Gripper control:
|
|
||||||
g - open
|
|
||||||
h - close
|
|
||||||
|
|
||||||
Other:
|
|
||||||
1 - Go to init pose
|
|
||||||
2 - Go to home pose
|
|
||||||
3 - Resave home pose
|
|
||||||
q - Quit
|
|
||||||
"""
|
|
||||||
|
|
||||||
|
|
||||||
def vels(speed, turn):
|
|
||||||
return "currently:\tspeed: %s\tchange percent: %s " % (speed, turn)
|
|
||||||
|
|
||||||
|
|
||||||
class Raw(object):
|
|
||||||
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)
|
|
||||||
|
|
||||||
|
|
||||||
def teleop_keyboard():
|
|
||||||
rospy.init_node("teleop_keyboard")
|
|
||||||
|
|
||||||
model = 1
|
|
||||||
speed = rospy.get_param("~speed", 50)
|
|
||||||
change_percent = rospy.get_param("~change_percent", 5)
|
|
||||||
|
|
||||||
change_angle = 180 * change_percent / 100
|
|
||||||
change_len = 250 * change_percent / 100
|
|
||||||
|
|
||||||
rospy.wait_for_service("get_joint_angles")
|
|
||||||
rospy.wait_for_service("set_joint_angles")
|
|
||||||
rospy.wait_for_service("get_joint_coords")
|
|
||||||
rospy.wait_for_service("set_joint_coords")
|
|
||||||
rospy.wait_for_service("switch_gripper_status")
|
|
||||||
print("service ready.")
|
|
||||||
try:
|
|
||||||
get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
|
|
||||||
set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
|
|
||||||
get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
|
||||||
set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
|
|
||||||
switch_gripper = rospy.ServiceProxy(
|
|
||||||
"switch_gripper_status", GripperStatus)
|
|
||||||
except:
|
|
||||||
print("start error ...")
|
|
||||||
exit(1)
|
|
||||||
|
|
||||||
init_pose = [0, 0, 0, 0, 0, 0, 0, speed]
|
|
||||||
home_pose = [0, 0, 0, -90, 0, 90, 0, speed]
|
|
||||||
|
|
||||||
rsp = set_angles(*home_pose)
|
|
||||||
|
|
||||||
while True:
|
|
||||||
res = get_coords()
|
|
||||||
if res.x > 1:
|
|
||||||
break
|
|
||||||
time.sleep(0.1)
|
|
||||||
|
|
||||||
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
|
|
||||||
print('init_coords:', record_coords)
|
|
||||||
|
|
||||||
try:
|
|
||||||
print(msg)
|
|
||||||
print(vels(speed, change_percent))
|
|
||||||
# Keyboard keys call different motion functions. 键盘按键调用不同的运动功能
|
|
||||||
while 1:
|
|
||||||
try:
|
|
||||||
# print("\r current coords: %s" % record_coords, end="")
|
|
||||||
with Raw(sys.stdin):
|
|
||||||
key = sys.stdin.read(1)
|
|
||||||
if key == "q":
|
|
||||||
break
|
|
||||||
elif key in ["w", "W"]:
|
|
||||||
record_coords[0] += change_len
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["s", "S"]:
|
|
||||||
record_coords[0] -= change_len
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["a", "A"]:
|
|
||||||
record_coords[1] -= change_len
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["d", "D"]:
|
|
||||||
record_coords[1] += change_len
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["z", "Z"]:
|
|
||||||
record_coords[2] -= change_len
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["x", "X"]:
|
|
||||||
record_coords[2] += change_len
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["u", "U"]:
|
|
||||||
record_coords[3] += change_angle
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["j", "J"]:
|
|
||||||
record_coords[3] -= change_angle
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["i", "I"]:
|
|
||||||
record_coords[4] += change_angle
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["k", "K"]:
|
|
||||||
record_coords[4] -= change_angle
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["o", "O"]:
|
|
||||||
record_coords[5] += change_angle
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["l", "L"]:
|
|
||||||
record_coords[5] -= change_angle
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["g", "G"]:
|
|
||||||
switch_gripper(True)
|
|
||||||
elif key in ["h", "H"]:
|
|
||||||
switch_gripper(False)
|
|
||||||
elif key == "1":
|
|
||||||
rsp = set_angles(*init_pose)
|
|
||||||
elif key in "2":
|
|
||||||
rsp = set_angles(*home_pose)
|
|
||||||
time.sleep(3)
|
|
||||||
res = get_coords()
|
|
||||||
time.sleep(0.1)
|
|
||||||
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
|
|
||||||
print('home_coords:', record_coords)
|
|
||||||
elif key in "3":
|
|
||||||
rep = get_angles()
|
|
||||||
home_pose[0] = rep.joint_1
|
|
||||||
home_pose[1] = rep.joint_2
|
|
||||||
home_pose[2] = rep.joint_3
|
|
||||||
home_pose[3] = rep.joint_4
|
|
||||||
home_pose[5] = rep.joint_5
|
|
||||||
else:
|
|
||||||
continue
|
|
||||||
|
|
||||||
except Exception as e:
|
|
||||||
# print(e)
|
|
||||||
continue
|
|
||||||
|
|
||||||
except Exception as e:
|
|
||||||
print(e)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
try:
|
|
||||||
teleop_keyboard()
|
|
||||||
except rospy.ROSInterruptException:
|
|
||||||
pass
|
|
||||||
Loading…
Add table
Reference in a new issue