kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-05 19:47:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

update and fix mercury_a1 ros

Browse source
This commit is contained in:
wangWking 2024-01-11 16:26:10 +08:00
parent ea1af4e1d9
commit a12eacca6b
4 changed files with 84 additions and 64 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

59
Mercury/mercury_a1/scripts/follow_display.py
View file

@ -60,42 +60,45 @@ def talker():
while not rospy.is_shutdown():
joint_state_send.header.stamp = rospy.Time.now()
angles = mc.get_angles()
data_list = []
for index, value in enumerate(angles):
radians = math.radians(value)
data_list.append(radians)
try:
angles = mc.get_angles()
data_list = []
for index, value in enumerate(angles):
radians = math.radians(value)
data_list.append(radians)
# rospy.loginfo('{}'.format(data_list))
joint_state_send.position = data_list
# rospy.loginfo('{}'.format(data_list))
joint_state_send.position = data_list
pub.publish(joint_state_send)
pub.publish(joint_state_send)
coords = mc.get_coords()
coords = mc.get_coords()
# marker
marker_.header.stamp = rospy.Time.now()
marker_.type = marker_.SPHERE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker
marker_.header.stamp = rospy.Time.now()
marker_.type = marker_.SPHERE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial.标记位置初始
# print(coords)
if not coords:
coords = [0, 0, 0, 0, 0, 0]
rospy.loginfo("error [101]: can not get coord values")
# marker position initial.标记位置初始
# print(coords)
if not coords:
coords = [0, 0, 0, 0, 0, 0]
rospy.loginfo("error [101]: can not get coord values")
marker_.pose.position.x = coords[1] / 1000 * -1
marker_.pose.position.y = coords[0] / 1000
marker_.pose.position.z = coords[2] / 1000
marker_.pose.position.x = coords[1] / 1000 * -1
marker_.pose.position.y = coords[0] / 1000
marker_.pose.position.z = coords[2] / 1000
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
rate.sleep()
except Exception as e:
pass
if __name__ == "__main__":

41
Mercury/mercury_a1/scripts/listen_real.py
View file

@ -39,26 +39,29 @@ def talker():
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))
try:
# 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()
# publish angles.发布角度
joint_state_send.header.stamp = rospy.Time.now()
joint_state_send.position = radians_list
pub.publish(joint_state_send)
rate.sleep()
except Exception as e:
pass
if __name__ == "__main__":

22
Mercury/mercury_a1/scripts/teleop_keyboard.py
View file

@ -83,16 +83,24 @@ def teleop_keyboard():
rsp = set_angles(*home_pose)
while True:
res = get_coords()
if res.x > 1:
break
time.sleep(0.1)
# 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)
# record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
# print('init_coords:', record_coords)
try:
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)
print(msg)
print(vels(speed, change_percent))
# Keyboard keys call different motion functions. 键盘按键调用不同的运动功能

26
Mercury/mercury_a1_communication/scripts/mercury_services.py
View file

@ -95,11 +95,14 @@ def set_angles(req):
def get_angles(req):
"""get angles,获取角度"""
if mc:
lock = acquire("/tmp/mercury_lock")
angles = mc.get_angles()
release(lock)
return GetAnglesResponse(*angles)
try:
if mc:
lock = acquire("/tmp/mercury_lock")
angles = mc.get_angles()
release(lock)
return GetAnglesResponse(*angles)
except Exception as e:
pass
def set_coords(req):
@ -123,11 +126,14 @@ def set_coords(req):
def get_coords(req):
if mc:
lock = acquire("/tmp/mercury_lock")
coords = mc.get_coords()
release(lock)
return GetCoordsResponse(*coords)
try:
if mc:
lock = acquire("/tmp/mercury_lock")
coords = mc.get_coords()
release(lock)
return GetCoordsResponse(*coords)
except Exception as e:
pass
def switch_status(req):