mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
update mercury ros
This commit is contained in:
wangWking
parent
84b01b404e
commit
9f87b2c31b
8 changed files with 113 additions and 117 deletions
|
|
@ -43,7 +43,7 @@ def listener():
|
|||
print(port, baud)
|
||||
mc = Mercury(port, baud)
|
||||
time.sleep(0.05)
|
||||
mc.set_free_mode(1)
|
||||
mc.set_fresh_mode(1)
|
||||
time.sleep(0.05)
|
||||
|
||||
# spin() simply keeps python from exiting until this node is stopped
|
||||
|
|
|
|||
|
|
@ -57,7 +57,9 @@ def create_handle():
|
|||
baud = rospy.get_param("~baud")
|
||||
rospy.loginfo("%s,%s" % (port, baud))
|
||||
mc = Mercury(port, baud)
|
||||
|
||||
time.sleep(0.05)
|
||||
mc.set_fresh_mode(1)
|
||||
time.sleep(0.05)
|
||||
|
||||
def create_services():
|
||||
rospy.Service("set_joint_angles", SetAngles, set_angles)
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@ Passable parameters:
|
|||
port: serial prot string. Defaults is '/dev/ttyAMA1'
|
||||
baud: serial prot baudrate. Defaults is 115200.
|
||||
"""
|
||||
import time
|
||||
import math
|
||||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
|
|
@ -39,7 +40,9 @@ def listener():
|
|||
baud = rospy.get_param("~baud", 115200)
|
||||
print(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()只是阻止python退出，直到该节点停止
|
||||
print("spin ...")
|
||||
|
|
|
|||
|
|
@ -289,9 +289,9 @@ Visualization Manager:
|
|||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.4097958207130432
|
||||
Pitch: 0.3747957646846771
|
||||
Target Frame: <Fixed Frame>
|
||||
Yaw: 6.260410308837891
|
||||
Yaw: 0.002224991098046303
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#!/usr/bin/env python3
|
||||
import time
|
||||
import math
|
||||
import traceback
|
||||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
|
|
@ -16,12 +17,11 @@ def talker():
|
|||
port1 = rospy.get_param("~port1", "/dev/ttyTHS1")
|
||||
port2 = rospy.get_param("~port2", "/dev/ttyS0")
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
print("port1: {}, baud: {}\n".format(port1, baud))
|
||||
print("port2: {}, baud: {}\n".format(port2, baud))
|
||||
print("left arm: {}, baud: {}\n".format(port1, baud))
|
||||
print("right arm: {}, baud: {}\n".format(port2, baud))
|
||||
try:
|
||||
# left arm
|
||||
l = Mercury(port1, baud)
|
||||
time.sleep(0.02)
|
||||
# right arm
|
||||
r = Mercury(port2, baud)
|
||||
except Exception as e:
|
||||
|
|
@ -35,9 +35,9 @@ def talker():
|
|||
)
|
||||
exit(1)
|
||||
l.release_all_servos()
|
||||
time.sleep(0.02)
|
||||
time.sleep(0.05)
|
||||
r.release_all_servos()
|
||||
time.sleep(0.1)
|
||||
time.sleep(0.05)
|
||||
print("Rlease all servos over.\n")
|
||||
|
||||
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||
|
|
@ -78,82 +78,74 @@ def talker():
|
|||
print("publishing ...")
|
||||
while not rospy.is_shutdown():
|
||||
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)
|
||||
|
||||
print('left: {}'.format(left_angles))
|
||||
print('right: {}'.format(right_angles))
|
||||
print('eye: {}'.format(eye_angle))
|
||||
print('head: {}'.format(head_angle))
|
||||
print('body: {}'.format(body_angle))
|
||||
|
||||
all_angles = left_angles + right_angles + [eye_angle] + [head_angle] + [body_angle]
|
||||
data_list = []
|
||||
for index, value in enumerate(all_angles):
|
||||
radians = math.radians(value)
|
||||
data_list.append(radians)
|
||||
|
||||
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)
|
||||
|
||||
print('left:', left_angles)
|
||||
print('right:', right_angles)
|
||||
print('eye:', eye_angle)
|
||||
print('head:', head_angle)
|
||||
print('body:', body_angle)
|
||||
|
||||
all_angles = left_angles + right_angles + eye_angle + head_angle + body_angle
|
||||
data_list = []
|
||||
for index, value in enumerate(all_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)
|
||||
left_coords = l.get_coords()
|
||||
|
||||
right_coords = r.get_coords()
|
||||
|
||||
eye_coords = r.get_angle(11)
|
||||
|
||||
head_coords = r.get_angle(12)
|
||||
|
||||
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
|
||||
|
||||
left_coords = l.get_coords()
|
||||
|
||||
right_coords = r.get_coords()
|
||||
|
||||
eye_coords = r.get_angle(11)
|
||||
|
||||
head_coords = r.get_angle(12)
|
||||
|
||||
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 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 = left_coords[1] / 1000 * -1
|
||||
marker_.pose.position.y = left_coords[0] / 1000
|
||||
marker_.pose.position.z = left_coords[2] / 1000
|
||||
|
||||
marker_.pose.position.x = left_coords[1] / 1000 * -1
|
||||
marker_.pose.position.y = left_coords[0] / 1000
|
||||
marker_.pose.position.z = left_coords[2] / 1000
|
||||
|
||||
time.sleep(0.02)
|
||||
|
||||
marker_.pose.position.x = right_coords[1] / 1000 * -1
|
||||
marker_.pose.position.y = right_coords[0] / 1000
|
||||
marker_.pose.position.z = right_coords[2] / 1000
|
||||
|
||||
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_.pose.position.x = right_coords[1] / 1000 * -1
|
||||
marker_.pose.position.y = right_coords[0] / 1000
|
||||
marker_.pose.position.z = right_coords[2] / 1000
|
||||
|
||||
marker_.pose.position.x = eye_coords[0] / 1000 * -1
|
||||
marker_.pose.position.x = head_coords[0] / 1000 * -1
|
||||
marker_.pose.position.x = body_coords[0] / 1000 * -1
|
||||
|
||||
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:
|
||||
e = traceback.format_exc()
|
||||
print(str(e))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
This file is [slider_control.launch] related script.
|
||||
Passable parameters:
|
||||
port: serial prot string. Defaults is '/dev/ttyAMA1'
|
||||
baud: serial prot baudrate. Defaults is 115200.
|
||||
port1: Left arm serial port string. Default is "/dev/ttyTHS1"
|
||||
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 time
|
||||
|
|
@ -15,10 +16,10 @@ from sensor_msgs.msg import JointState
|
|||
|
||||
from pymycobot.mercury import Mercury
|
||||
|
||||
# left arm port
|
||||
# left arm
|
||||
l = None
|
||||
|
||||
# right arm port
|
||||
# right arm
|
||||
r = None
|
||||
|
||||
|
||||
|
|
@ -29,14 +30,15 @@ def callback(data):
|
|||
for index, value in enumerate(data.position):
|
||||
radians_to_angles = round(math.degrees(value), 2)
|
||||
data_list.append(radians_to_angles)
|
||||
print('data_list:', data_list)
|
||||
|
||||
print('data_list: {}'.format(data_list))
|
||||
left_arm = data_list[:7]
|
||||
right_arm = data_list[7:-3]
|
||||
middle_arm = data_list[-3:]
|
||||
|
||||
print('left_arm:', left_arm)
|
||||
print('right_arm:', right_arm)
|
||||
print('middle_arm:', middle_arm)
|
||||
print('left_angles: {}'.format(left_arm))
|
||||
print('right_angles: {}'.format(right_arm))
|
||||
print('middle_arm: {}'.format(middle_arm))
|
||||
|
||||
l.send_angles(left_arm, 25)
|
||||
time.sleep(0.02)
|
||||
|
|
@ -58,13 +60,13 @@ def listener():
|
|||
port1 = rospy.get_param("~port1", "/dev/ttyTHS1")
|
||||
port2 = rospy.get_param("~port2", "/dev/ttyS0")
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
print(port1, baud)
|
||||
print(port2, baud)
|
||||
print('left arm: {}, {}'.format(port1, baud))
|
||||
print('right arm: {}, {}'.format(port2, baud))
|
||||
l = Mercury(port1, baud)
|
||||
r = Mercury(port2, baud)
|
||||
time.sleep(0.05)
|
||||
l.set_free_mode(1)
|
||||
r.set_free_mode(1)
|
||||
l.set_fresh_mode(1)
|
||||
r.set_fresh_mode(1)
|
||||
time.sleep(0.05)
|
||||
# spin() simply keeps python from exiting until this node is stopped
|
||||
# spin()只是阻止python退出，直到该节点停止
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
This file is [slider_control.launch] related script.
|
||||
Passable parameters:
|
||||
port: serial prot string. Defaults is '/dev/ttyAMA1'
|
||||
baud: serial prot baudrate. Defaults is 115200.
|
||||
port1: Left arm serial port string. Default is "/dev/ttyTHS1"
|
||||
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 time
|
||||
|
|
@ -16,10 +17,10 @@ from sensor_msgs.msg import JointState
|
|||
|
||||
from pymycobot.mercury import Mercury
|
||||
|
||||
# left arm port
|
||||
# left arm
|
||||
l = None
|
||||
|
||||
# right arm port
|
||||
# right arm
|
||||
r = None
|
||||
|
||||
|
||||
|
|
@ -30,25 +31,20 @@ def callback(data):
|
|||
for index, value in enumerate(data.position):
|
||||
radians_to_angles = round(math.degrees(value), 2)
|
||||
data_list.append(radians_to_angles)
|
||||
print('data_list:', data_list)
|
||||
print('data_list: {}'.format(data_list))
|
||||
left_arm = data_list[:7]
|
||||
right_arm = data_list[7:-3]
|
||||
middle_arm = data_list[-3:]
|
||||
|
||||
print('left_arm:', left_arm)
|
||||
print('right_arm:', right_arm)
|
||||
print('middle_arm:', middle_arm)
|
||||
print('left_angles: {}'.format(left_arm))
|
||||
print('right_angles: {}'.format(right_arm))
|
||||
print('middle_angles: {}'.format(middle_arm))
|
||||
|
||||
l.send_angles(left_arm, 25)
|
||||
time.sleep(0.02)
|
||||
r.send_angles(right_arm, 25)
|
||||
time.sleep(0.02)
|
||||
r.send_angle(11, middle_arm[0], 25)
|
||||
time.sleep(0.02)
|
||||
r.send_angle(12, middle_arm[1], 25)
|
||||
time.sleep(0.02)
|
||||
r.send_angle(13, middle_arm[2], 25)
|
||||
time.sleep(0.02)
|
||||
|
||||
|
||||
def listener():
|
||||
|
|
@ -56,17 +52,18 @@ def listener():
|
|||
rospy.init_node("control_slider", anonymous=True)
|
||||
|
||||
rospy.Subscriber("joint_states", JointState, callback)
|
||||
port1 = rospy.get_param("~port1", "/dev/ttyS0")
|
||||
port2 = rospy.get_param("~port2", "/dev/ttyTHS1")
|
||||
port1 = rospy.get_param("~port1", "/dev/ttyTHS1")
|
||||
port2 = rospy.get_param("~port2", "/dev/ttyS0")
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
print(port1, baud)
|
||||
print(port2, baud)
|
||||
print('left arm: {}, {}'.format(port1, baud))
|
||||
print('right arm: {}, {}'.format(port2, baud))
|
||||
l = Mercury(port1, baud)
|
||||
r = Mercury(port2, baud)
|
||||
time.sleep(0.05)
|
||||
l.set_free_mode(1)
|
||||
r.set_free_mode(1)
|
||||
l.set_fresh_mode(1)
|
||||
r.set_fresh_mode(1)
|
||||
time.sleep(0.05)
|
||||
|
||||
# spin() simply keeps python from exiting until this node is stopped
|
||||
# spin()只是阻止python退出，直到该节点停止
|
||||
print("spin ...")
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
|
||||
<xacro:property name="width" value=".2" />
|
||||
|
||||
<!-- 底部 -->
|
||||
<!-- base -->
|
||||
<link name="base">
|
||||
<visual>
|
||||
<geometry>
|
||||
|
|
@ -19,7 +19,7 @@
|
|||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- 腰部 -->
|
||||
<!-- body -->
|
||||
<link name="link_body">
|
||||
<visual>
|
||||
<geometry>
|
||||
|
|
@ -35,7 +35,7 @@
|
|||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- 屏幕头部 -->
|
||||
<!-- screen head -->
|
||||
<link name="link_head">
|
||||
<visual>
|
||||
<geometry>
|
||||
|
|
@ -51,19 +51,19 @@
|
|||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- 摄像头 -->
|
||||
<!-- camera -->
|
||||
<link name="link_eye">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mercury_b1/head_eye.dae"/>
|
||||
</geometry>
|
||||
<origin xyz = "0 0 -0.0 " rpy = " 0 0 3.14159"/>
|
||||
<origin xyz = "0 0 -0.0 " rpy = " 0 0 -0.6981"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mercury_b1/head_eye.dae"/>
|
||||
</geometry>
|
||||
<origin xyz = "0 0 -0.1 " rpy = " 0 0 3.14159"/>
|
||||
<origin xyz = "0 0 -0.1 " rpy = " 0 0 -0.6981"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
|
|
@ -392,10 +392,10 @@
|
|||
|
||||
<joint name="eye" type="revolute">
|
||||
<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"/>
|
||||
<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 name="head" type="revolute">
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue