kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-14 09:00:53 +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(): while not rospy.is_shutdown():
joint_state_send.header.stamp = rospy.Time.now() joint_state_send.header.stamp = rospy.Time.now()
angles = mc.get_angles() try:
data_list = [] angles = mc.get_angles()
for index, value in enumerate(angles): data_list = []
radians = math.radians(value) for index, value in enumerate(angles):
data_list.append(radians) radians = math.radians(value)
data_list.append(radians)
# rospy.loginfo('{}'.format(data_list)) # rospy.loginfo('{}'.format(data_list))
joint_state_send.position = 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
marker_.header.stamp = rospy.Time.now() marker_.header.stamp = rospy.Time.now()
marker_.type = marker_.SPHERE marker_.type = marker_.SPHERE
marker_.action = marker_.ADD marker_.action = marker_.ADD
marker_.scale.x = 0.04 marker_.scale.x = 0.04
marker_.scale.y = 0.04 marker_.scale.y = 0.04
marker_.scale.z = 0.04 marker_.scale.z = 0.04
# marker position initial.标记位置初始 # marker position initial.标记位置初始
# print(coords) # print(coords)
if not coords: if not coords:
coords = [0, 0, 0, 0, 0, 0] coords = [0, 0, 0, 0, 0, 0]
rospy.loginfo("error [101]: can not get coord values") rospy.loginfo("error [101]: can not get coord values")
marker_.pose.position.x = coords[1] / 1000 * -1 marker_.pose.position.x = coords[1] / 1000 * -1
marker_.pose.position.y = coords[0] / 1000 marker_.pose.position.y = coords[0] / 1000
marker_.pose.position.z = coords[2] / 1000 marker_.pose.position.z = coords[2] / 1000
marker_.color.a = 1.0 marker_.color.a = 1.0
marker_.color.g = 1.0 marker_.color.g = 1.0
pub_marker.publish(marker_) pub_marker.publish(marker_)
rate.sleep() rate.sleep()
except Exception as e:
pass
if __name__ == "__main__": if __name__ == "__main__":

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

@ -39,26 +39,29 @@ def talker():
rospy.loginfo("start loop ...") rospy.loginfo("start loop ...")
while not rospy.is_shutdown(): while not rospy.is_shutdown():
# get real angles from server.从服务器获得真实的角度。 try:
res = func() # get real angles from server.从服务器获得真实的角度。
if res.joint_1 == res.joint_2 == res.joint_3 == 0.0: res = func()
continue if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
radians_list = [ continue
res.joint_1 * (math.pi / 180), radians_list = [
res.joint_2 * (math.pi / 180), res.joint_1 * (math.pi / 180),
res.joint_3 * (math.pi / 180), res.joint_2 * (math.pi / 180),
res.joint_4 * (math.pi / 180), res.joint_3 * (math.pi / 180),
res.joint_5 * (math.pi / 180), res.joint_4 * (math.pi / 180),
res.joint_6 * (math.pi / 180), res.joint_5 * (math.pi / 180),
res.joint_7 * (math.pi / 180), res.joint_6 * (math.pi / 180),
] res.joint_7 * (math.pi / 180),
rospy.loginfo("res: {}".format(radians_list)) ]
rospy.loginfo("res: {}".format(radians_list))
# publish angles.发布角度 # publish angles.发布角度
joint_state_send.header.stamp = rospy.Time.now() joint_state_send.header.stamp = rospy.Time.now()
joint_state_send.position = radians_list joint_state_send.position = radians_list
pub.publish(joint_state_send) pub.publish(joint_state_send)
rate.sleep() rate.sleep()
except Exception as e:
pass
if __name__ == "__main__": 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) rsp = set_angles(*home_pose)
while True: # while True:
res = get_coords() # res = get_coords()
if res.x > 1: # if res.x > 1:
break # break
time.sleep(0.1) # time.sleep(0.1)
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model] # record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
print('init_coords:', record_coords) # print('init_coords:', record_coords)
try: 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(msg)
print(vels(speed, change_percent)) print(vels(speed, change_percent))
# Keyboard keys call different motion functions. 键盘按键调用不同的运动功能 # 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): def get_angles(req):
"""get angles,获取角度""" """get angles,获取角度"""
if mc: try:
lock = acquire("/tmp/mercury_lock") if mc:
angles = mc.get_angles() lock = acquire("/tmp/mercury_lock")
release(lock) angles = mc.get_angles()
return GetAnglesResponse(*angles) release(lock)
return GetAnglesResponse(*angles)
except Exception as e:
pass
def set_coords(req): def set_coords(req):
@ -123,11 +126,14 @@ def set_coords(req):
def get_coords(req): def get_coords(req):
if mc: try:
lock = acquire("/tmp/mercury_lock") if mc:
coords = mc.get_coords() lock = acquire("/tmp/mercury_lock")
release(lock) coords = mc.get_coords()
return GetCoordsResponse(*coords) release(lock)
return GetCoordsResponse(*coords)
except Exception as e:
pass
def switch_status(req): def switch_status(req):