mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
280M5 repaired and optimized ros1 keyboard control movement effect, and changed to topic communication
This commit is contained in:
wangWking
parent
138cec92b5
commit
312501e434
11 changed files with 316 additions and 145 deletions
|
|
@ -26,6 +26,7 @@ catkin_install_python(PROGRAMS
|
|||
scripts/teleop_keyboard.py
|
||||
scripts/listen_real.py
|
||||
scripts/listen_real_of_topic.py
|
||||
scripts/listen_real_of_topic_gripper.py
|
||||
scripts/simple_gui.py
|
||||
scripts/follow_display_gripper.py
|
||||
scripts/slider_control_gripper.py
|
||||
|
|
|
|||
|
|
@ -14,10 +14,10 @@
|
|||
<!-- Show in Rviz ，显示在Rviz-->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<include file="$(find mycobot_communication)/launch/communication_service.launch">
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic.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="mycobot_280" type="listen_real.py" />
|
||||
<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -16,13 +16,13 @@
|
|||
<!-- Show in Rviz ，显示在Rviz-->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<include file="$(find mycobot_communication)/launch/communication_service.launch">
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
</include>
|
||||
<!-- listen and pub the real angles ，监听并发布真实角度-->
|
||||
<!-- <node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_gripper.py" /> -->
|
||||
<node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_gripper.py" output="screen">
|
||||
<node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_of_topic_gripper.py" output="screen">
|
||||
<param name="port" value="$(arg port)" />
|
||||
<param name="baud" value="$(arg baud)" />
|
||||
</node>
|
||||
|
|
|
|||
|
|
@ -14,10 +14,10 @@
|
|||
<!-- Show in Rviz ，显示在Rviz-->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<include file="$(find mycobot_communication)/launch/communication_service.launch">
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic.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="mycobot_280" type="listen_real.py" />
|
||||
<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -51,7 +51,7 @@ class Listener(object):
|
|||
data.joint_5 * (math.pi / 180),
|
||||
data.joint_6 * (math.pi / 180),
|
||||
]
|
||||
rospy.loginfo("res: {}".format(radians_list))
|
||||
# rospy.loginfo("res: {}".format(radians_list))
|
||||
|
||||
joint_state_send.position = radians_list
|
||||
self.pub.publish(joint_state_send)
|
||||
|
|
|
|||
72
mycobot_280/mycobot_280/scripts/listen_real_of_topic_gripper.py
Executable file
72
mycobot_280/mycobot_280/scripts/listen_real_of_topic_gripper.py
Executable file
|
|
@ -0,0 +1,72 @@
|
|||
#!/usr/bin/env python3
|
||||
# encoding:utf-8
|
||||
|
||||
import math
|
||||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
from mycobot_communication.msg import MycobotAngles, MycobotGetGripperValue
|
||||
|
||||
|
||||
class Listener(object):
|
||||
def __init__(self):
|
||||
super(Listener, self).__init__()
|
||||
|
||||
rospy.loginfo("start ...")
|
||||
rospy.init_node("real_listener_gripper", anonymous=True)
|
||||
self.gripper_angle = 0.0
|
||||
# init publisher.初始化发布者
|
||||
self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||
# init subscriber.初始化订阅者
|
||||
self.sub = rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.callback)
|
||||
self.sub2 = rospy.Subscriber("mycobot/gripper_angle_real", MycobotGetGripperValue, self.gripper_callback)
|
||||
rospy.spin()
|
||||
|
||||
def gripper_callback(self, data):
|
||||
"""夹爪角度更新"""
|
||||
self.gripper_angle = data.gripper_angle
|
||||
|
||||
def callback(self, data):
|
||||
"""`mycobot/angles_real` subscriber callback method.
|
||||
|
||||
Args:
|
||||
data (MycobotAngles): callback argument.
|
||||
"""
|
||||
# ini publisher object. 初始化发布者对象
|
||||
joint_state_send = JointState()
|
||||
joint_state_send.header = Header()
|
||||
|
||||
joint_state_send.name = [
|
||||
"joint2_to_joint1",
|
||||
"joint3_to_joint2",
|
||||
"joint4_to_joint3",
|
||||
"joint5_to_joint4",
|
||||
"joint6_to_joint5",
|
||||
"joint6output_to_joint6",
|
||||
"gripper_controller",
|
||||
]
|
||||
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),
|
||||
self.gripper_angle * math.pi / 180.0,
|
||||
]
|
||||
# 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,49 +1,63 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- coding:utf-8 -*-
|
||||
from __future__ import print_function
|
||||
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus, PumpStatus
|
||||
import rospy
|
||||
import sys
|
||||
import select
|
||||
import time
|
||||
import termios
|
||||
import tty
|
||||
import time
|
||||
import math
|
||||
import rospy
|
||||
|
||||
import roslib
|
||||
from mycobot_communication.msg import (
|
||||
MycobotAngles,
|
||||
MycobotCoords,
|
||||
MycobotSetAngles,
|
||||
MycobotSetCoords,
|
||||
MycobotGripperStatus,
|
||||
MycobotPumpStatus,
|
||||
)
|
||||
|
||||
# Terminal output prompt information. 终端输出提示信息
|
||||
msg = """\
|
||||
Mycobot Teleop Keyboard Controller
|
||||
---------------------------
|
||||
Movimg options(control coordinations [x,y,z,rx,ry,rz]):
|
||||
w(x+)
|
||||
Mycobot Teleop Keyboard Controller (ROS1 - Topic Version)
|
||||
---------------------------------------------------------
|
||||
Movement (Cartesian):
|
||||
w (x+)
|
||||
a (y+) s (x-) d (y-)
|
||||
z (z-) x (z+)
|
||||
|
||||
a(y-) s(x-) d(y+)
|
||||
Rotation (Euler angles):
|
||||
u (rx+) i (ry+) o (rz+)
|
||||
j (rx-) k (ry-) l (rz-)
|
||||
|
||||
z(z-) x(z+)
|
||||
Movement Step:
|
||||
+ : Increase movement step size
|
||||
- : Decrease movement step size
|
||||
|
||||
u(rx+) i(ry+) o(rz+)
|
||||
j(rx-) k(ry-) l(rz-)
|
||||
Gripper:
|
||||
g - open h - close
|
||||
|
||||
Gripper control:
|
||||
g - open
|
||||
h - close
|
||||
|
||||
Pump control:
|
||||
b - open
|
||||
m - close
|
||||
Pump:
|
||||
b - open m - close
|
||||
|
||||
Other:
|
||||
1 - Go to init pose
|
||||
2 - Go to home pose
|
||||
3 - Resave home pose
|
||||
3 - Save current pose as home
|
||||
q - Quit
|
||||
"""
|
||||
|
||||
COORD_LIMITS = {
|
||||
'x': (-281.45, 281.45),
|
||||
'y': (-281.45, 281.45),
|
||||
'z': (-70, 412.67),
|
||||
'rx': (-180, 180),
|
||||
'ry': (-180, 180),
|
||||
'rz': (-180, 180)
|
||||
}
|
||||
|
||||
def vels(speed, turn):
|
||||
return "currently:\tspeed: %s\tchange percent: %s " % (speed, turn)
|
||||
|
||||
|
||||
class Raw(object):
|
||||
def __init__(self, stream):
|
||||
self.stream = stream
|
||||
|
|
@ -57,129 +71,139 @@ class Raw(object):
|
|||
termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty)
|
||||
|
||||
|
||||
def teleop_keyboard():
|
||||
rospy.init_node("teleop_keyboard")
|
||||
class MycobotTeleopTopic:
|
||||
def __init__(self):
|
||||
rospy.init_node("teleop_keyboard_topic")
|
||||
|
||||
model = 0
|
||||
speed = rospy.get_param("~speed", 50)
|
||||
change_percent = rospy.get_param("~change_percent", 5)
|
||||
self.speed = rospy.get_param("~speed", 50)
|
||||
self.model = 1
|
||||
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
|
||||
|
||||
change_angle = 180 * change_percent / 100
|
||||
change_len = 250 * change_percent / 100
|
||||
# 当前坐标和角度
|
||||
self.curr_coords = [0] * 6
|
||||
self.curr_angles = [0] * 6
|
||||
|
||||
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")
|
||||
rospy.wait_for_service("switch_pump_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)
|
||||
switch_pump = rospy.ServiceProxy(
|
||||
"switch_pump_status", PumpStatus)
|
||||
except:
|
||||
print("start error ...")
|
||||
exit(1)
|
||||
# 保存坐标作为目标值
|
||||
self.record_coords = None
|
||||
self.home_pose = [0, 8, -127, 40, 0, 0]
|
||||
|
||||
init_pose = [0, 0, 0, 0, 0, 0, speed]
|
||||
home_pose = [0, 8, -127, 40, 0, 0, speed]
|
||||
# 订阅器
|
||||
rospy.Subscriber("mycobot/coords_real", MycobotCoords, self.coords_callback)
|
||||
rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.angles_callback)
|
||||
|
||||
# rsp = set_angles(*init_pose)
|
||||
# 发布器
|
||||
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)
|
||||
self.pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=1)
|
||||
|
||||
while True:
|
||||
res = get_coords()
|
||||
if res.x > 1:
|
||||
break
|
||||
time.sleep(0.1)
|
||||
rospy.loginfo("Waiting to receive current coordinates...")
|
||||
while self.curr_coords == [0] * 6 and not rospy.is_shutdown():
|
||||
time.sleep(0.1)
|
||||
|
||||
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
|
||||
print(record_coords)
|
||||
|
||||
try:
|
||||
self.record_coords = list(self.curr_coords)
|
||||
print(msg)
|
||||
print(vels(speed, change_percent))
|
||||
# Keyboard keys call different motion functions. 键盘按键调用不同的运动功能
|
||||
while 1:
|
||||
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):
|
||||
self.curr_coords = [msg.x, msg.y, msg.z, msg.rx, msg.ry, msg.rz]
|
||||
|
||||
def angles_callback(self, msg):
|
||||
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):
|
||||
goal = MycobotSetCoords()
|
||||
goal.x, goal.y, goal.z, goal.rx, goal.ry, goal.rz = self.record_coords
|
||||
goal.speed = self.speed
|
||||
goal.model = self.model
|
||||
self.coords_pub.publish(goal)
|
||||
|
||||
def send_angles(self, angles):
|
||||
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):
|
||||
while not rospy.is_shutdown():
|
||||
try:
|
||||
# print("\r current coords: %s" % record_coords, end="")
|
||||
with Raw(sys.stdin):
|
||||
key = sys.stdin.read(1)
|
||||
if key == "q":
|
||||
except Exception:
|
||||
continue
|
||||
|
||||
try:
|
||||
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 in ["b", "B"]:
|
||||
switch_pump(True, 2, 5)
|
||||
elif key in ["m", "M"]:
|
||||
switch_pump(False, 2, 5)
|
||||
elif key == "1":
|
||||
rsp = set_angles(*init_pose)
|
||||
elif key in "2":
|
||||
rsp = set_angles(*home_pose)
|
||||
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
|
||||
elif key == '+':
|
||||
self.change_percent = min(self.change_percent + 1, 20) # 最大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) # 最小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
|
||||
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
|
||||
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
|
||||
elif key in 'gG':
|
||||
self.gripper_pub.publish(MycobotGripperStatus(Status=True))
|
||||
elif key in 'hH':
|
||||
self.gripper_pub.publish(MycobotGripperStatus(Status=False))
|
||||
elif key in 'bB':
|
||||
self.pump_pub.publish(MycobotPumpStatus(Status=True, Pin1=2, Pin2=5))
|
||||
elif key in 'mM':
|
||||
self.pump_pub.publish(MycobotPumpStatus(Status=False, Pin1=2, Pin2=5))
|
||||
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
|
||||
|
||||
# 范围检查
|
||||
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:
|
||||
# print(e)
|
||||
rospy.logwarn("Execution failed: {}".format(e))
|
||||
continue
|
||||
|
||||
except Exception as e:
|
||||
print(e)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
teleop_keyboard()
|
||||
teleop = MycobotTeleopTopic()
|
||||
teleop.run()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
|
|
|||
|
|
@ -59,6 +59,7 @@ add_message_files(FILES
|
|||
MycobotSetEndType.msg
|
||||
MycobotSetFreshMode.msg
|
||||
MycobotSetToolReference.msg
|
||||
MycobotGetGripperValue.msg
|
||||
)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
|
|
|
|||
2
mycobot_communication/msg/MycobotGetGripperValue.msg
Normal file
2
mycobot_communication/msg/MycobotGetGripperValue.msg
Normal file
|
|
@ -0,0 +1,2 @@
|
|||
int32 gripper_angle
|
||||
|
||||
|
|
@ -163,11 +163,15 @@ def toggle_pump(req):
|
|||
if mc:
|
||||
lock = acquire("/tmp/mycobot_lock")
|
||||
if req.Status:
|
||||
mc.set_basic_output(2, 0)
|
||||
mc.set_basic_output(5, 0)
|
||||
time.sleep(0.05)
|
||||
else:
|
||||
mc.set_basic_output(2, 1)
|
||||
mc.set_basic_output(5, 1)
|
||||
time.sleep(0.05)
|
||||
mc.set_basic_output(2, 0)
|
||||
time.sleep(0.05)
|
||||
mc.set_basic_output(2, 1)
|
||||
time.sleep(0.05)
|
||||
release(lock)
|
||||
|
||||
|
||||
|
|
@ -178,11 +182,11 @@ robot_msg = """
|
|||
MyCobot Status
|
||||
--------------------------------
|
||||
Joint Limit:
|
||||
joint 1: -170 ~ +170
|
||||
joint 2: -170 ~ +170
|
||||
joint 3: -170 ~ +170
|
||||
joint 4: -170 ~ +170
|
||||
joint 5: -170 ~ +170
|
||||
joint 1: -168 ~ +168
|
||||
joint 2: -135 ~ +135
|
||||
joint 3: -150 ~ +150
|
||||
joint 4: -145 ~ +145
|
||||
joint 5: -165 ~ +165
|
||||
joint 6: -180 ~ +180
|
||||
|
||||
Connect Status: %s
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@ from mycobot_communication.msg import (
|
|||
MycobotSetEndType,
|
||||
MycobotSetFreshMode,
|
||||
MycobotSetToolReference,
|
||||
MycobotGetGripperValue,
|
||||
)
|
||||
from std_msgs.msg import UInt8
|
||||
import pymycobot
|
||||
|
|
@ -76,14 +77,31 @@ class Watcher:
|
|||
os.kill(self.child, signal.SIGKILL)
|
||||
except OSError:
|
||||
pass
|
||||
robot_msg = """
|
||||
MyCobot Status
|
||||
--------------------------------
|
||||
Joint Limit:
|
||||
joint 1: -168 ~ +168
|
||||
joint 2: -135 ~ +135
|
||||
joint 3: -150 ~ +150
|
||||
joint 4: -145 ~ +145
|
||||
joint 5: -165 ~ +165
|
||||
joint 6: -180 ~ +180
|
||||
|
||||
Connect Status: %s
|
||||
|
||||
Servo Infomation: %s
|
||||
|
||||
Servo Temperature: %s
|
||||
|
||||
Atom Version: %s
|
||||
"""
|
||||
|
||||
class MycobotTopics(object):
|
||||
def __init__(self):
|
||||
super(MycobotTopics, self).__init__()
|
||||
|
||||
rospy.init_node("mycobot_topics")
|
||||
rospy.loginfo("start ...")
|
||||
rospy.init_node("mycobot_topics")
|
||||
port = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1])
|
||||
if not port:
|
||||
port = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1])
|
||||
|
|
@ -91,6 +109,7 @@ class MycobotTopics(object):
|
|||
rospy.loginfo("%s,%s" % (port, baud))
|
||||
self.mc = MyCobot280(port, baud)
|
||||
self.lock = threading.Lock()
|
||||
self.output_robot_message()
|
||||
|
||||
def start(self):
|
||||
pa = threading.Thread(target=self.pub_real_angles)
|
||||
|
|
@ -103,6 +122,7 @@ class MycobotTopics(object):
|
|||
sfm = threading.Thread(target=self.sub_fresh_mode_status)
|
||||
set = threading.Thread(target=self.sub_end_type_status)
|
||||
str = threading.Thread(target=self.sub_set_tool_reference)
|
||||
sgv = threading.Thread(target=self.sub_real_gripper_value)
|
||||
|
||||
pa.setDaemon(True)
|
||||
pa.start()
|
||||
|
|
@ -123,6 +143,8 @@ class MycobotTopics(object):
|
|||
set.start()
|
||||
str.setDaemon(True)
|
||||
str.start()
|
||||
sgv.setDaemon(True)
|
||||
sgv.start()
|
||||
|
||||
pa.join()
|
||||
pb.join()
|
||||
|
|
@ -134,6 +156,7 @@ class MycobotTopics(object):
|
|||
sfm.join()
|
||||
set.join()
|
||||
str.join()
|
||||
sgv.join()
|
||||
|
||||
def pub_real_angles(self):
|
||||
"""Publish real angle"""
|
||||
|
|
@ -212,6 +235,22 @@ class MycobotTopics(object):
|
|||
)
|
||||
rospy.spin()
|
||||
|
||||
def sub_real_gripper_value(self):
|
||||
"""Get Gripper Value"""
|
||||
pub = rospy.Publisher("mycobot/gripper_angle_real",
|
||||
MycobotGetGripperValue, queue_size=5)
|
||||
ma = MycobotGetGripperValue()
|
||||
while not rospy.is_shutdown():
|
||||
with self.lock:
|
||||
try:
|
||||
gripper_value = self.mc.get_gripper_value()
|
||||
if gripper_value:
|
||||
ma.gripper_angle = gripper_value
|
||||
pub.publish(ma)
|
||||
except Exception as e:
|
||||
rospy.logerr(f"SerialException: {e}")
|
||||
time.sleep(0.25)
|
||||
|
||||
def sub_gripper_status(self):
|
||||
"""Subscribe to Gripper Status"""
|
||||
"""订阅夹爪状态"""
|
||||
|
|
@ -229,11 +268,15 @@ class MycobotTopics(object):
|
|||
def sub_pump_status(self):
|
||||
def callback(data):
|
||||
if data.Status:
|
||||
self.mc.set_basic_output(data.Pin1, 0)
|
||||
self.mc.set_basic_output(data.Pin2, 0)
|
||||
time.sleep(0.05)
|
||||
else:
|
||||
self.mc.set_basic_output(data.Pin1, 1)
|
||||
self.mc.set_basic_output(data.Pin2, 1)
|
||||
time.sleep(0.05)
|
||||
self.mc.set_basic_output(data.Pin1, 0)
|
||||
time.sleep(0.05)
|
||||
self.mc.set_basic_output(data.PIn1, 1)
|
||||
time.sleep(0.05)
|
||||
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/pump_status", MycobotPumpStatus, callback=callback
|
||||
|
|
@ -279,6 +322,30 @@ class MycobotTopics(object):
|
|||
rospy.spin()
|
||||
|
||||
|
||||
def output_robot_message(self):
|
||||
connect_status = False
|
||||
servo_infomation = "unknown"
|
||||
servo_temperature = "unknown"
|
||||
atom_version = "unknown"
|
||||
|
||||
if self.mc:
|
||||
cn = self.mc.is_controller_connected()
|
||||
if cn == 1:
|
||||
connect_status = True
|
||||
time.sleep(0.1)
|
||||
si = self.mc.is_all_servo_enable()
|
||||
if si == 1:
|
||||
servo_infomation = "all connected"
|
||||
version = self.mc.get_system_version()
|
||||
if version:
|
||||
atom_version = version
|
||||
|
||||
print(
|
||||
robot_msg % (connect_status, servo_infomation,
|
||||
servo_temperature, atom_version)
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
Watcher()
|
||||
mc_topics = MycobotTopics()
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue