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 mercury ros

Browse source
This commit is contained in:
wangWking 2023-12-13 17:51:55 +08:00
parent 84b01b404e
commit 9f87b2c31b
8 changed files with 113 additions and 117 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
Mercury/mercury_a1/scripts/slider_control.py
View file

@ -43,7 +43,7 @@ def listener():
print(port, baud) print(port, baud)
mc = Mercury(port, baud) mc = Mercury(port, baud)
time.sleep(0.05) time.sleep(0.05)
mc.set_free_mode(1) mc.set_fresh_mode(1)
time.sleep(0.05) time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped # spin() simply keeps python from exiting until this node is stopped

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

@ -57,7 +57,9 @@ def create_handle():
baud = rospy.get_param("~baud") baud = rospy.get_param("~baud")
rospy.loginfo("%s,%s" % (port, baud)) rospy.loginfo("%s,%s" % (port, baud))
mc = Mercury(port, baud) mc = Mercury(port, baud)
time.sleep(0.05)
mc.set_fresh_mode(1)
time.sleep(0.05)
def create_services(): def create_services():
rospy.Service("set_joint_angles", SetAngles, set_angles) rospy.Service("set_joint_angles", SetAngles, set_angles)

5
Mercury/mercury_a1_moveit/scripts/sync_plan.py
View file

@ -9,6 +9,7 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyAMA1' port: serial prot string. Defaults is '/dev/ttyAMA1'
baud: serial prot baudrate. Defaults is 115200. baud: serial prot baudrate. Defaults is 115200.
""" """
import time
import math import math
import rospy import rospy
from sensor_msgs.msg import JointState from sensor_msgs.msg import JointState
@ -39,7 +40,9 @@ def listener():
baud = rospy.get_param("~baud", 115200) baud = rospy.get_param("~baud", 115200)
print(port, baud) print(port, baud)
mc = Mercury(port, baud) mc = Mercury(port, baud)
time.sleep(0.05)
mc.set_fresh_mode(1)
time.sleep(0.05)
# 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退出，直到该节点停止
print("spin ...") print("spin ...")

4
Mercury/mercury_b1/config/mercury_b1.rviz
View file

@ -289,9 +289,9 @@ Visualization Manager:
Invert Z Axis: false Invert Z Axis: false
Name: Current View Name: Current View
Near Clip Distance: 0.009999999776482582 Near Clip Distance: 0.009999999776482582
Pitch: 0.4097958207130432 Pitch: 0.3747957646846771
Target Frame: <Fixed Frame> Target Frame: <Fixed Frame>
Yaw: 6.260410308837891 Yaw: 0.002224991098046303
Saved: ~ Saved: ~
Window Geometry: Window Geometry:
Displays: Displays:

122
Mercury/mercury_b1/scripts/follow_display.py
View file

@ -1,6 +1,7 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
import time import time
import math import math
import traceback
import rospy import rospy
from sensor_msgs.msg import JointState from sensor_msgs.msg import JointState
from std_msgs.msg import Header from std_msgs.msg import Header
@ -16,12 +17,11 @@ def talker():
port1 = rospy.get_param("~port1", "/dev/ttyTHS1") port1 = rospy.get_param("~port1", "/dev/ttyTHS1")
port2 = rospy.get_param("~port2", "/dev/ttyS0") 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("left arm: {}, baud: {}\n".format(port1, baud))
print("port2: {}, baud: {}\n".format(port2, baud)) print("right arm: {}, baud: {}\n".format(port2, baud))
try: try:
# left arm # left arm
l = Mercury(port1, baud) l = Mercury(port1, baud)
time.sleep(0.02)
# right arm # right arm
r = Mercury(port2, baud) r = Mercury(port2, baud)
except Exception as e: except Exception as e:
@ -35,9 +35,9 @@ def talker():
) )
exit(1) exit(1)
l.release_all_servos() l.release_all_servos()
time.sleep(0.02) time.sleep(0.05)
r.release_all_servos() r.release_all_servos()
time.sleep(0.1) time.sleep(0.05)
print("Rlease all servos over.\n") print("Rlease all servos over.\n")
pub = rospy.Publisher("joint_states", JointState, queue_size=10) pub = rospy.Publisher("joint_states", JointState, queue_size=10)
@ -78,82 +78,74 @@ def talker():
print("publishing ...") print("publishing ...")
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()
try:
left_angles = l.get_angles()
right_angles = r.get_angles()
eye_angle = r.get_angle(11)
head_angle = r.get_angle(12)
body_angle = r.get_angle(13)
left_angles = l.get_angles() print('left: {}'.format(left_angles))
right_angles = r.get_angles() print('right: {}'.format(right_angles))
eye_angle = r.get_angle(11) print('eye: {}'.format(eye_angle))
head_angle = r.get_angle(12) print('head: {}'.format(head_angle))
body_angle = r.get_angle(13) print('body: {}'.format(body_angle))
print('left:', left_angles) all_angles = left_angles + right_angles + [eye_angle] + [head_angle] + [body_angle]
print('right:', right_angles) data_list = []
print('eye:', eye_angle) for index, value in enumerate(all_angles):
print('head:', head_angle) radians = math.radians(value)
print('body:', body_angle) data_list.append(radians)
all_angles = left_angles + right_angles + eye_angle + head_angle + body_angle # rospy.loginfo('{}'.format(data_list))
data_list = [] joint_state_send.position = data_list
for index, value in enumerate(all_angles):
radians = math.radians(value)
data_list.append(radians)
# rospy.loginfo('{}'.format(data_list)) pub.publish(joint_state_send)
joint_state_send.position = data_list
pub.publish(joint_state_send) left_coords = l.get_coords()
left_coords = l.get_coords() right_coords = r.get_coords()
right_coords = r.get_coords() eye_coords = r.get_angle(11)
eye_coords = r.get_angle(11) head_coords = r.get_angle(12)
head_coords = r.get_angle(12) body_coords = r.get_angle(13)
body_coords = r.get_angle(13) # 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 left_coords:
left_coords = [0, 0, 0, 0, 0, 0]
rospy.loginfo("error [101]: can not get coord values")
# marker marker_.pose.position.x = left_coords[1] / 1000 * -1
marker_.header.stamp = rospy.Time.now() marker_.pose.position.y = left_coords[0] / 1000
marker_.type = marker_.SPHERE marker_.pose.position.z = left_coords[2] / 1000
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial.标记位置初始 marker_.pose.position.x = right_coords[1] / 1000 * -1
# print(coords) marker_.pose.position.y = right_coords[0] / 1000
# if not coords: marker_.pose.position.z = right_coords[2] / 1000
# coords = [0, 0, 0, 0, 0, 0]
# rospy.loginfo("error [101]: can not get coord values")
marker_.pose.position.x = left_coords[1] / 1000 * -1 marker_.pose.position.x = eye_coords[0] / 1000 * -1
marker_.pose.position.y = left_coords[0] / 1000 marker_.pose.position.x = head_coords[0] / 1000 * -1
marker_.pose.position.z = left_coords[2] / 1000 marker_.pose.position.x = body_coords[0] / 1000 * -1
time.sleep(0.02) marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
marker_.pose.position.x = right_coords[1] / 1000 * -1 rate.sleep()
marker_.pose.position.y = right_coords[0] / 1000 except Exception as e:
marker_.pose.position.z = right_coords[2] / 1000 e = traceback.format_exc()
print(str(e))
time.sleep(0.02)
marker_.pose.position.x = eye_coords[0] / 1000 * -1
time.sleep(0.02)
marker_.pose.position.x = head_coords[0] / 1000 * -1
time.sleep(0.02)
marker_.pose.position.x = body_coords[0] / 1000 * -1
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__": if __name__ == "__main__":

26
Mercury/mercury_b1/scripts/slider_control.py
View file

@ -5,8 +5,9 @@ This file obtains the joint angle of the manipulator in ROS,
and then sends it directly to the real manipulator using `pymycobot` API. and then sends it directly to the real manipulator using `pymycobot` API.
This file is [slider_control.launch] related script. This file is [slider_control.launch] related script.
Passable parameters: Passable parameters:
port: serial prot string. Defaults is '/dev/ttyAMA1' port1: Left arm serial port string. Default is "/dev/ttyTHS1"
baud: serial prot baudrate. Defaults is 115200. port2: Right arm serial port string. Default is "/dev/ttyS0"
Baud rate: Left and right arm serial port baud rate. The default value is 115200.
""" """
import math import math
import time import time
@ -15,10 +16,10 @@ from sensor_msgs.msg import JointState
from pymycobot.mercury import Mercury from pymycobot.mercury import Mercury
# left arm port # left arm
l = None l = None
# right arm port # right arm
r = None r = None
@ -29,14 +30,15 @@ def callback(data):
for index, value in enumerate(data.position): for index, value in enumerate(data.position):
radians_to_angles = round(math.degrees(value), 2) radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles) data_list.append(radians_to_angles)
print('data_list:', data_list)
print('data_list: {}'.format(data_list))
left_arm = data_list[:7] left_arm = data_list[:7]
right_arm = data_list[7:-3] right_arm = data_list[7:-3]
middle_arm = data_list[-3:] middle_arm = data_list[-3:]
print('left_arm:', left_arm) print('left_angles: {}'.format(left_arm))
print('right_arm:', right_arm) print('right_angles: {}'.format(right_arm))
print('middle_arm:', middle_arm) print('middle_arm: {}'.format(middle_arm))
l.send_angles(left_arm, 25) l.send_angles(left_arm, 25)
time.sleep(0.02) time.sleep(0.02)
@ -58,13 +60,13 @@ def listener():
port1 = rospy.get_param("~port1", "/dev/ttyTHS1") port1 = rospy.get_param("~port1", "/dev/ttyTHS1")
port2 = rospy.get_param("~port2", "/dev/ttyS0") port2 = rospy.get_param("~port2", "/dev/ttyS0")
baud = rospy.get_param("~baud", 115200) baud = rospy.get_param("~baud", 115200)
print(port1, baud) print('left arm: {}, {}'.format(port1, baud))
print(port2, baud) print('right arm: {}, {}'.format(port2, baud))
l = Mercury(port1, baud) l = Mercury(port1, baud)
r = Mercury(port2, baud) r = Mercury(port2, baud)
time.sleep(0.05) time.sleep(0.05)
l.set_free_mode(1) l.set_fresh_mode(1)
r.set_free_mode(1) r.set_fresh_mode(1)
time.sleep(0.05) time.sleep(0.05)
# 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退出，直到该节点停止

35
Mercury/mercury_b1_moveit/scripts/sync_plan.py
View file

@ -6,8 +6,9 @@ This file obtains the joint angle of the manipulator in ROS,
and then sends it directly to the real manipulator using `pymycobot` API. and then sends it directly to the real manipulator using `pymycobot` API.
This file is [slider_control.launch] related script. This file is [slider_control.launch] related script.
Passable parameters: Passable parameters:
port: serial prot string. Defaults is '/dev/ttyAMA1' port1: Left arm serial port string. Default is "/dev/ttyTHS1"
baud: serial prot baudrate. Defaults is 115200. port2: Right arm serial port string. Default is "/dev/ttyS0"
Baud rate: Left and right arm serial port baud rate. The default value is 115200.
""" """
import math import math
import time import time
@ -16,10 +17,10 @@ from sensor_msgs.msg import JointState
from pymycobot.mercury import Mercury from pymycobot.mercury import Mercury
# left arm port # left arm
l = None l = None
# right arm port # right arm
r = None r = None
@ -30,25 +31,20 @@ def callback(data):
for index, value in enumerate(data.position): for index, value in enumerate(data.position):
radians_to_angles = round(math.degrees(value), 2) radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles) data_list.append(radians_to_angles)
print('data_list:', data_list) print('data_list: {}'.format(data_list))
left_arm = data_list[:7] left_arm = data_list[:7]
right_arm = data_list[7:-3] right_arm = data_list[7:-3]
middle_arm = data_list[-3:] middle_arm = data_list[-3:]
print('left_arm:', left_arm) print('left_angles: {}'.format(left_arm))
print('right_arm:', right_arm) print('right_angles: {}'.format(right_arm))
print('middle_arm:', middle_arm) print('middle_angles: {}'.format(middle_arm))
l.send_angles(left_arm, 25) l.send_angles(left_arm, 25)
time.sleep(0.02)
r.send_angles(right_arm, 25) r.send_angles(right_arm, 25)
time.sleep(0.02)
r.send_angle(11, middle_arm[0], 25) r.send_angle(11, middle_arm[0], 25)
time.sleep(0.02)
r.send_angle(12, middle_arm[1], 25) r.send_angle(12, middle_arm[1], 25)
time.sleep(0.02)
r.send_angle(13, middle_arm[2], 25) r.send_angle(13, middle_arm[2], 25)
time.sleep(0.02)
def listener(): def listener():
@ -56,17 +52,18 @@ def listener():
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('left arm: {}, {}'.format(port1, baud))
print(port2, baud) print('right arm: {}, {}'.format(port2, baud))
l = Mercury(port1, baud) l = Mercury(port1, baud)
r = Mercury(port2, baud) r = Mercury(port2, baud)
time.sleep(0.05) time.sleep(0.05)
l.set_free_mode(1) l.set_fresh_mode(1)
r.set_free_mode(1) r.set_fresh_mode(1)
time.sleep(0.05) time.sleep(0.05)
# 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退出，直到该节点停止
print("spin ...") print("spin ...")

16
mycobot_description/urdf/mercury_b1/mercury_b1.urdf
View file

@ -3,7 +3,7 @@
<xacro:property name="width" value=".2" /> <xacro:property name="width" value=".2" />
<!-- 底部 --> <!-- base -->
<link name="base"> <link name="base">
<visual> <visual>
<geometry> <geometry>
@ -19,7 +19,7 @@
</collision> </collision>
</link> </link>
<!-- 腰部 --> <!-- body -->
<link name="link_body"> <link name="link_body">
<visual> <visual>
<geometry> <geometry>
@ -35,7 +35,7 @@
</collision> </collision>
</link> </link>
<!-- 屏幕头部 --> <!-- screen head -->
<link name="link_head"> <link name="link_head">
<visual> <visual>
<geometry> <geometry>
@ -51,19 +51,19 @@
</collision> </collision>
</link> </link>
<!-- 摄像头 --> <!-- camera -->
<link name="link_eye"> <link name="link_eye">
<visual> <visual>
<geometry> <geometry>
<mesh filename="package://mycobot_description/urdf/mercury_b1/head_eye.dae"/> <mesh filename="package://mycobot_description/urdf/mercury_b1/head_eye.dae"/>
</geometry> </geometry>
<origin xyz = "0 0 -0.0 " rpy = " 0 0 3.14159"/> <origin xyz = "0 0 -0.0 " rpy = " 0 0 -0.6981"/>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
<mesh filename="package://mycobot_description/urdf/mercury_b1/head_eye.dae"/> <mesh filename="package://mycobot_description/urdf/mercury_b1/head_eye.dae"/>
</geometry> </geometry>
<origin xyz = "0 0 -0.1 " rpy = " 0 0 3.14159"/> <origin xyz = "0 0 -0.1 " rpy = " 0 0 -0.6981"/>
</collision> </collision>
</link> </link>
@ -392,10 +392,10 @@
<joint name="eye" type="revolute"> <joint name="eye" type="revolute">
<axis xyz=" 0 0 1"/> <axis xyz=" 0 0 1"/>
<limit effort = "1000.0" lower = "0" upper = "0.8377" velocity = "0"/> <limit effort = "1000.0" lower = "0" upper = "0.83" velocity = "0"/>
<parent link="link_head"/> <parent link="link_head"/>
<child link="link_eye"/> <child link="link_eye"/>
<origin xyz= " 0.03 0 -0.08 " rpy = "1.5708 0 3.14"/> <origin xyz= " 0.03 0 -0.08 " rpy = "1.5708 0 0"/>
</joint> </joint>
<joint name="head" type="revolute"> <joint name="head" type="revolute">