mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
Add English annotation
This commit is contained in:
wangWking
parent
4dcf049350
commit
180f31f02c
60 changed files with 392 additions and 206 deletions
|
|
@ -1,4 +1,5 @@
|
|||
<launch>
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
|
||||
<arg name="gui" default="true" />
|
||||
|
|
@ -10,6 +11,7 @@
|
|||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
</include>
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
||||
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
|
||||
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
||||
|
|
|
|||
|
|
@ -1,7 +1,8 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
|
||||
<arg name="gui" default="false" />
|
||||
|
|
@ -10,17 +11,17 @@
|
|||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF -->
|
||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<!-- Show in Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<!-- mycobot-topics -->
|
||||
<!-- mycobot-topics ，mycobot-话题-->
|
||||
<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 -->
|
||||
<!-- listen and pub the real angles ，监听并发布真实的角度-->
|
||||
<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
||||
|
|
|
|||
|
|
@ -3,9 +3,10 @@
|
|||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
|
||||
<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
|
||||
</node>
|
||||
<!-- show in Rviza,显示在Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
|
|
@ -8,9 +9,9 @@
|
|||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF -->
|
||||
<!-- Combinejoin values to TF ,将值合并到TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<!-- Show in Rviz -->
|
||||
<!-- 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">
|
||||
|
|
|
|||
|
|
@ -1,14 +1,14 @@
|
|||
<launch>
|
||||
<!-- <arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="baud" default="115200" /> -->
|
||||
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
|
||||
<arg name="gui" default="true" />
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF -->
|
||||
<!-- Combinejoin values to TF，将值合并到TF -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
|
||||
<!-- <param name="use_gui" value="$(arg gui)" /> -->
|
||||
|
|
@ -19,6 +19,6 @@
|
|||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
</node> -->
|
||||
<!-- Show in Rviz -->
|
||||
<!-- Show in Rviz ，显示在Rviz-->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
|
|
@ -8,14 +9,15 @@
|
|||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF -->
|
||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<!-- Show in Rviz -->
|
||||
<!-- 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">
|
||||
<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" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -11,6 +11,6 @@
|
|||
<param name="use_gui" value="$(arg gui)" />
|
||||
<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
|
||||
</node>
|
||||
<!-- Show in Rviz -->
|
||||
<!-- Show in Rviz ，显示在Rviz-->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ class ImageConverter:
|
|||
)
|
||||
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
|
||||
self.camera_matrix = None
|
||||
# subscriber, listen wether has img come in.
|
||||
# subscriber, listen wether has img come in. 订阅者，监听是否有img
|
||||
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
|
||||
|
||||
def callback(self, data):
|
||||
|
|
@ -37,7 +37,7 @@ class ImageConverter:
|
|||
pose to transforming.
|
||||
"""
|
||||
try:
|
||||
# trans `rgb` to `gbr` for opencv.
|
||||
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
|
||||
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
|
|
@ -45,7 +45,7 @@ class ImageConverter:
|
|||
focal_length = size[1]
|
||||
center = [size[1] / 2, size[0] / 2]
|
||||
if self.camera_matrix is None:
|
||||
# calc the camera matrix, if don't have.
|
||||
# calc the camera matrix, if don't have.如果没有，则计算相机矩阵
|
||||
self.camera_matrix = np.array(
|
||||
[
|
||||
[focal_length, 0, center[0]],
|
||||
|
|
@ -55,18 +55,18 @@ class ImageConverter:
|
|||
dtype=np.float32,
|
||||
)
|
||||
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
|
||||
# detect aruco marker.
|
||||
# detect aruco marker.检测 aruco 标记
|
||||
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
|
||||
corners, ids = ret[0], ret[1]
|
||||
# process marker data.
|
||||
# process marker data.处理标记数据
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
# print('corners:', corners, 'ids:', ids)
|
||||
|
||||
# detect marker pose.
|
||||
# detect marker pose. 检测marker位姿。
|
||||
# argument:
|
||||
# marker corners
|
||||
# marker size (meter)
|
||||
# marker corners,标记角
|
||||
# marker size (meter),标记尺寸（米）
|
||||
ret = cv.aruco.estimatePoseSingleMarkers(
|
||||
corners, 0.05, self.camera_matrix, self.dist_coeffs
|
||||
)
|
||||
|
|
@ -75,7 +75,7 @@ class ImageConverter:
|
|||
|
||||
print("rvec:", rvec, "tvec:", tvec)
|
||||
|
||||
# just select first one detected marker.
|
||||
# just select first one detected marker.只需选择第一个检测到的标记。
|
||||
for i in range(rvec.shape[0]):
|
||||
cv.aruco.drawDetectedMarkers(cv_image, corners)
|
||||
cv.aruco.drawAxis(
|
||||
|
|
@ -90,14 +90,14 @@ class ImageConverter:
|
|||
xyz = tvec[0, 0, :]
|
||||
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
|
||||
|
||||
# get quaternion for ros.
|
||||
# get quaternion for ros. 为ros获取四元数
|
||||
euler = rvec[0, 0, :]
|
||||
tf_change = tf.transformations.quaternion_from_euler(
|
||||
euler[0], euler[1], euler[2]
|
||||
)
|
||||
print("tf_change:", tf_change)
|
||||
|
||||
# trans pose according [joint1]
|
||||
# trans pose according [joint1]，根据 [joint1] 变换姿势
|
||||
self.br.sendTransform(
|
||||
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
|
||||
)
|
||||
|
|
|
|||
|
|
@ -5,18 +5,19 @@ from visualization_msgs.msg import Marker
|
|||
import time
|
||||
import os
|
||||
|
||||
# 与 mycobot 通信的消息类型
|
||||
# Type of message communicated with mycobot，与 mycobot 通信的消息类型
|
||||
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
|
||||
|
||||
|
||||
rospy.init_node("gipper_subscriber", anonymous=True)
|
||||
|
||||
# Control the topic of mycobot, followed by angle, coordinates, gripper
|
||||
# 控制 mycobot 的 topic，依次是角度、坐标、夹爪
|
||||
angle_pub = rospy.Publisher("mycobot/angles_goal",
|
||||
MycobotSetAngles, queue_size=5)
|
||||
coord_pub = rospy.Publisher("mycobot/coords_goal",
|
||||
MycobotSetCoords, queue_size=5)
|
||||
# Judging equipment: ttyUSB* is M5；ttyACM* is wio
|
||||
# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
|
||||
robot = os.popen("ls /dev/ttyUSB*").readline()
|
||||
|
||||
if "dev" in robot:
|
||||
|
|
@ -27,27 +28,28 @@ else:
|
|||
pump_pub = rospy.Publisher("mycobot/pump_status",
|
||||
MycobotPumpStatus, queue_size=5)
|
||||
|
||||
# 实例化消息对象
|
||||
# instantiate the message object，实例化消息对象
|
||||
angles = MycobotSetAngles()
|
||||
coords = MycobotSetCoords()
|
||||
pump = MycobotPumpStatus()
|
||||
|
||||
# 与 mycobot 真实位置的偏差值
|
||||
# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值
|
||||
x_offset = -20
|
||||
y_offset = 20
|
||||
z_offset = 110
|
||||
|
||||
# With this variable limit, the fetching behavior is only done once
|
||||
# 通过该变量限制，抓取行为只做一次
|
||||
flag = False
|
||||
|
||||
# 为了后面比较二维码是否移动
|
||||
# In order to compare whether the QR code moves later,为了后面比较二维码是否移动
|
||||
temp_x = temp_y = temp_z = 0.0
|
||||
|
||||
temp_time = time.time()
|
||||
|
||||
|
||||
def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
||||
"""发布坐标"""
|
||||
"""Post coordinates,发布坐标"""
|
||||
coords.x = x
|
||||
coords.y = y
|
||||
coords.z = z
|
||||
|
|
@ -61,7 +63,7 @@ def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
|||
|
||||
|
||||
def pub_angles(a, b, c, d, e, f, sp):
|
||||
"""发布角度"""
|
||||
"""Publishing angle,发布角度"""
|
||||
angles.joint_1 = float(a)
|
||||
angles.joint_2 = float(b)
|
||||
angles.joint_3 = float(c)
|
||||
|
|
@ -73,7 +75,7 @@ def pub_angles(a, b, c, d, e, f, sp):
|
|||
|
||||
|
||||
def pub_pump(flag, Pin):
|
||||
"""发布夹爪状态"""
|
||||
"""Publish gripper status,发布夹爪状态"""
|
||||
pump.Status = flag
|
||||
pump.Pin1 = Pin[0]
|
||||
pump.Pin2 = Pin[1]
|
||||
|
|
@ -81,6 +83,7 @@ def pub_pump(flag, Pin):
|
|||
|
||||
|
||||
def target_is_moving(x, y, z):
|
||||
"""Determine whether the target moves"""
|
||||
"""判断目标是否移动"""
|
||||
count = 0
|
||||
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
|
||||
|
|
@ -94,18 +97,19 @@ def target_is_moving(x, y, z):
|
|||
|
||||
|
||||
def grippercallback(data):
|
||||
"""回调函数"""
|
||||
"""callback function,回调函数"""
|
||||
global flag, temp_x, temp_y, temp_z
|
||||
# rospy.loginfo('gripper_subscriber get date :%s', data)
|
||||
if flag:
|
||||
return
|
||||
|
||||
# 解析出坐标值
|
||||
# Parse out the coordinate value,解析出坐标值
|
||||
# pump length: 88mm
|
||||
x = float(format(data.pose.position.x * 1000, ".2f"))
|
||||
y = float(format(data.pose.position.y * 1000, ".2f"))
|
||||
z = float(format(data.pose.position.z * 1000, ".2f"))
|
||||
|
||||
# When the running time is less than 30s, or the target position is still changing, perform tracking behavior
|
||||
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
|
||||
if (
|
||||
time.time() - temp_time < 30
|
||||
|
|
@ -120,7 +124,7 @@ def grippercallback(data):
|
|||
|
||||
temp_x, temp_y, temp_z = x, y, z
|
||||
return
|
||||
else: # 表示目标处于静止状态，可以尝试抓取
|
||||
else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
|
||||
|
||||
print(x, y, z)
|
||||
|
||||
|
|
@ -178,7 +182,7 @@ def main():
|
|||
pub_pump(False, Pin)
|
||||
# time.sleep(2.5)
|
||||
|
||||
# mark 信息的订阅者
|
||||
# mark 信息的订阅者,subscribers to mark information
|
||||
rospy.Subscriber("visualization_marker", Marker,
|
||||
grippercallback, queue_size=1)
|
||||
|
||||
|
|
|
|||
|
|
@ -79,7 +79,7 @@ def talker():
|
|||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
|
||||
# marker position initial
|
||||
# marker position initial.标记位置初始
|
||||
# print(coords)
|
||||
if not coords:
|
||||
coords = [0, 0, 0, 0, 0, 0]
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@ def talker():
|
|||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
|
||||
# marker position initial
|
||||
# marker position initial,标记位置初始化
|
||||
marker_.pose.position.x = trans[0]
|
||||
marker_.pose.position.y = trans[1]
|
||||
marker_.pose.position.z = trans[2]
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ def talker():
|
|||
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||
rate = rospy.Rate(30) # 30hz
|
||||
|
||||
# pub joint state
|
||||
# pub joint state，发布关节状态
|
||||
joint_state_send = JointState()
|
||||
joint_state_send.header = Header()
|
||||
|
||||
|
|
@ -30,14 +30,14 @@ def talker():
|
|||
joint_state_send.velocity = [0]
|
||||
joint_state_send.effort = []
|
||||
|
||||
# waiting util server `get_joint_angles` enable.
|
||||
# waiting util server `get_joint_angles` enable.等待'get_joint_angles'服务启用
|
||||
rospy.loginfo("wait service")
|
||||
rospy.wait_for_service("get_joint_angles")
|
||||
func = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
||||
|
||||
rospy.loginfo("start loop ...")
|
||||
while not rospy.is_shutdown():
|
||||
# get real angles from server.
|
||||
# get real angles from server.从服务器获得真实的角度。
|
||||
res = func()
|
||||
if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
|
||||
continue
|
||||
|
|
@ -51,7 +51,7 @@ def talker():
|
|||
]
|
||||
rospy.loginfo("res: {}".format(radians_list))
|
||||
|
||||
# publish angles.
|
||||
# publish angles.发布角度
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
joint_state_send.position = radians_list
|
||||
pub.publish(joint_state_send)
|
||||
|
|
|
|||
|
|
@ -13,9 +13,9 @@ class Listener(object):
|
|||
|
||||
rospy.loginfo("start ...")
|
||||
rospy.init_node("real_listener_1", anonymous=True)
|
||||
# init publisher.
|
||||
# init publisher.初始化发布者
|
||||
self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||
# init subscriber.
|
||||
# init subscriber.初始化订阅者
|
||||
self.sub = rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.callback)
|
||||
rospy.spin()
|
||||
|
||||
|
|
@ -25,7 +25,7 @@ class Listener(object):
|
|||
Args:
|
||||
data (MycobotAngles): callback argument.
|
||||
"""
|
||||
# ini publisher object.
|
||||
# ini publisher object. 初始化发布者对象
|
||||
joint_state_send = JointState()
|
||||
joint_state_send.header = Header()
|
||||
|
||||
|
|
@ -41,7 +41,7 @@ class Listener(object):
|
|||
joint_state_send.effort = []
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
|
||||
# process callback data.
|
||||
# process callback data. 处理回调数据。
|
||||
radians_list = [
|
||||
data.joint_1 * (math.pi / 180),
|
||||
data.joint_2 * (math.pi / 180),
|
||||
|
|
|
|||
|
|
@ -10,47 +10,48 @@ from rospy import ServiceException
|
|||
class Window:
|
||||
def __init__(self, handle):
|
||||
self.win = handle
|
||||
self.win.resizable(0, 0) # 固定窗口大小
|
||||
self.win.resizable(0, 0) # fixed window size，固定窗口大小
|
||||
|
||||
self.model = 0
|
||||
self.speed = rospy.get_param("~speed", 50)
|
||||
|
||||
# 设置默认速度
|
||||
# set default speed，设置默认速度
|
||||
self.speed_d = tk.StringVar()
|
||||
self.speed_d.set(str(self.speed))
|
||||
# print(self.speed)
|
||||
self.connect_ser()
|
||||
|
||||
# 获取机械臂数据
|
||||
# Get the data of the robotic arm，获取机械臂数据
|
||||
self.record_coords = [0, 0, 0, 0, 0, 0, self.speed, self.model]
|
||||
self.res_angles = [0, 0, 0, 0, 0, 0, self.speed, self.model]
|
||||
self.get_date()
|
||||
|
||||
# get screen width and height
|
||||
# get screen width and height.获取屏幕宽度和高度
|
||||
self.ws = self.win.winfo_screenwidth() # width of the screen
|
||||
self.hs = self.win.winfo_screenheight() # height of the screen
|
||||
# calculate x and y coordinates for the Tk root window
|
||||
# 计算 Tk 根窗口的 x 和 y 坐标
|
||||
x = (self.ws / 2) - 190
|
||||
y = (self.hs / 2) - 250
|
||||
self.win.geometry("430x370+{}+{}".format(x, y))
|
||||
# 布局
|
||||
# layout,布局
|
||||
self.set_layout()
|
||||
# 输入部分
|
||||
# input section,输入部分
|
||||
self.need_input()
|
||||
# 展示部分
|
||||
# Show part,展示部分
|
||||
self.show_init()
|
||||
|
||||
# joint 设置按钮
|
||||
# Set the joint buttons 设置joint按钮
|
||||
tk.Button(self.frmLT, text="设置", width=5, command=self.get_joint_input).grid(
|
||||
row=6, column=1, sticky="w", padx=3, pady=2
|
||||
)
|
||||
|
||||
# coordination 设置按钮
|
||||
# coordination settings button,coordination 设置按钮
|
||||
tk.Button(self.frmRT, text="设置", width=5, command=self.get_coord_input).grid(
|
||||
row=6, column=1, sticky="w", padx=3, pady=2
|
||||
)
|
||||
|
||||
# 夹爪开关按钮
|
||||
# Gripper switch button,夹爪开关按钮
|
||||
tk.Button(self.frmLB, text="夹爪(开)", command=self.gripper_open, width=5).grid(
|
||||
row=1, column=0, sticky="w", padx=3, pady=50
|
||||
)
|
||||
|
|
@ -91,22 +92,22 @@ class Window:
|
|||
self.frmRT.grid(row=0, column=1, padx=2, pady=3)
|
||||
|
||||
def need_input(self):
|
||||
# 输入提示
|
||||
# input hint,输入提示
|
||||
tk.Label(self.frmLT, text="Joint 1 ").grid(row=0)
|
||||
tk.Label(self.frmLT, text="Joint 2 ").grid(row=1) # 第二行
|
||||
tk.Label(self.frmLT, text="Joint 2 ").grid(row=1) # the second row,第二行
|
||||
tk.Label(self.frmLT, text="Joint 3 ").grid(row=2)
|
||||
tk.Label(self.frmLT, text="Joint 4 ").grid(row=3)
|
||||
tk.Label(self.frmLT, text="Joint 5 ").grid(row=4)
|
||||
tk.Label(self.frmLT, text="Joint 6 ").grid(row=5)
|
||||
|
||||
tk.Label(self.frmRT, text=" x ").grid(row=0)
|
||||
tk.Label(self.frmRT, text=" y ").grid(row=1) # 第二行
|
||||
tk.Label(self.frmRT, text=" y ").grid(row=1) # the second row,第二行
|
||||
tk.Label(self.frmRT, text=" z ").grid(row=2)
|
||||
tk.Label(self.frmRT, text=" rx ").grid(row=3)
|
||||
tk.Label(self.frmRT, text=" ry ").grid(row=4)
|
||||
tk.Label(self.frmRT, text=" rz ").grid(row=5)
|
||||
|
||||
# 设置输入框的默认值
|
||||
# Set the default value of the input box,设置输入框的默认值
|
||||
self.j1_default = tk.StringVar()
|
||||
self.j1_default.set(self.res_angles[0])
|
||||
self.j2_default = tk.StringVar()
|
||||
|
|
@ -133,7 +134,7 @@ class Window:
|
|||
self.rz_default = tk.StringVar()
|
||||
self.rz_default.set(self.record_coords[5])
|
||||
|
||||
# joint 输入框
|
||||
# joint input box,joint 输入框
|
||||
self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default)
|
||||
self.J_1.grid(row=0, column=1, pady=3)
|
||||
self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default)
|
||||
|
|
@ -147,7 +148,7 @@ class Window:
|
|||
self.J_6 = tk.Entry(self.frmLT, textvariable=self.j6_default)
|
||||
self.J_6.grid(row=5, column=1, pady=3)
|
||||
|
||||
# coord 输入框
|
||||
# coord input box,coord 输入框
|
||||
self.x = tk.Entry(self.frmRT, textvariable=self.x_default)
|
||||
self.x.grid(row=0, column=1, pady=3, padx=0)
|
||||
self.y = tk.Entry(self.frmRT, textvariable=self.y_default)
|
||||
|
|
@ -161,11 +162,11 @@ class Window:
|
|||
self.rz = tk.Entry(self.frmRT, textvariable=self.rz_default)
|
||||
self.rz.grid(row=5, column=1, pady=3)
|
||||
|
||||
# 所有输入框，用于拿输入的数据
|
||||
# All input boxes, used to get the input data,所有输入框，用于拿输入的数据
|
||||
self.all_j = [self.J_1, self.J_2, self.J_3, self.J_4, self.J_5, self.J_6]
|
||||
self.all_c = [self.x, self.y, self.z, self.rx, self.ry, self.rz]
|
||||
|
||||
# 速度输入框
|
||||
# speed input box,速度输入框
|
||||
tk.Label(
|
||||
self.frmLB,
|
||||
text="speed",
|
||||
|
|
@ -174,9 +175,9 @@ class Window:
|
|||
self.get_speed.grid(row=0, column=1)
|
||||
|
||||
def show_init(self):
|
||||
# 显示
|
||||
# show,显示
|
||||
tk.Label(self.frmLC, text="Joint 1 ").grid(row=0)
|
||||
tk.Label(self.frmLC, text="Joint 2 ").grid(row=1) # 第二行
|
||||
tk.Label(self.frmLC, text="Joint 2 ").grid(row=1) # the second row,第二行
|
||||
tk.Label(self.frmLC, text="Joint 3 ").grid(row=2)
|
||||
tk.Label(self.frmLC, text="Joint 4 ").grid(row=3)
|
||||
tk.Label(self.frmLC, text="Joint 5 ").grid(row=4)
|
||||
|
|
@ -184,7 +185,7 @@ class Window:
|
|||
|
||||
# get数据
|
||||
|
||||
# ，展示出来
|
||||
# show,展示出来
|
||||
self.cont_1 = tk.StringVar(self.frmLC)
|
||||
self.cont_1.set(str(self.res_angles[0]) + "°")
|
||||
self.cont_2 = tk.StringVar(self.frmLC)
|
||||
|
|
@ -267,9 +268,9 @@ class Window:
|
|||
self.show_j6,
|
||||
]
|
||||
|
||||
# 显示
|
||||
# show,显示
|
||||
tk.Label(self.frmLC, text=" x ").grid(row=0, column=3)
|
||||
tk.Label(self.frmLC, text=" y ").grid(row=1, column=3) # 第二行
|
||||
tk.Label(self.frmLC, text=" y ").grid(row=1, column=3)
|
||||
tk.Label(self.frmLC, text=" z ").grid(row=2, column=3)
|
||||
tk.Label(self.frmLC, text=" rx ").grid(row=3, column=3)
|
||||
tk.Label(self.frmLC, text=" ry ").grid(row=4, column=3)
|
||||
|
|
@ -347,7 +348,7 @@ class Window:
|
|||
bg="white",
|
||||
).grid(row=5, column=4, padx=5, pady=5)
|
||||
|
||||
# mm 单位展示
|
||||
# mm， Unit show，单位展示
|
||||
self.unit = tk.StringVar()
|
||||
self.unit.set("mm")
|
||||
for i in range(6):
|
||||
|
|
@ -359,6 +360,7 @@ class Window:
|
|||
try:
|
||||
self.switch_gripper(True)
|
||||
except ServiceException:
|
||||
# Probably because the method has no return value, the service throws an unhandled error
|
||||
# 可能由于该方法没有返回值，服务抛出无法处理的错误
|
||||
pass
|
||||
|
||||
|
|
@ -369,6 +371,7 @@ class Window:
|
|||
pass
|
||||
|
||||
def get_coord_input(self):
|
||||
# Get the data input by coord and send it to the robotic arm
|
||||
# 获取 coord 输入的数据，发送给机械臂
|
||||
c_value = []
|
||||
for i in self.all_c:
|
||||
|
|
@ -387,6 +390,7 @@ class Window:
|
|||
self.show_j_date(c_value[:-2], "coord")
|
||||
|
||||
def get_joint_input(self):
|
||||
# Get the data input by the joint and send it to the robotic arm
|
||||
# 获取joint输入的数据，发送给机械臂
|
||||
j_value = []
|
||||
for i in self.all_j:
|
||||
|
|
@ -405,7 +409,7 @@ class Window:
|
|||
# return j_value,c_value,speed
|
||||
|
||||
def get_date(self):
|
||||
# 拿机械臂的数据，用于展示
|
||||
# Take the data of the robotic arm for display.拿机械臂的数据，用于展示
|
||||
t = time.time()
|
||||
while time.time() - t < 2:
|
||||
self.res = self.get_coords()
|
||||
|
|
@ -443,7 +447,7 @@ class Window:
|
|||
|
||||
# def send_input(self,dates):
|
||||
def show_j_date(self, date, way=""):
|
||||
# 展示数据
|
||||
# Show data,展示数据
|
||||
if way == "coord":
|
||||
for i, j in zip(date, self.coord_all):
|
||||
# print(i)
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@ def listener():
|
|||
mc = MyCobot(port, baud)
|
||||
|
||||
# spin() simply keeps python from exiting until this node is stopped
|
||||
# spin()只是阻止python退出，直到该节点停止
|
||||
print("spin ...")
|
||||
rospy.spin()
|
||||
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ import time
|
|||
|
||||
import roslib
|
||||
|
||||
|
||||
# Terminal output prompt information. 终端输出提示信息
|
||||
msg = """\
|
||||
Mycobot Teleop Keyboard Controller
|
||||
---------------------------
|
||||
|
|
@ -97,6 +97,7 @@ def teleop_keyboard():
|
|||
try:
|
||||
print(msg)
|
||||
print(vels(speed, change_percent))
|
||||
# Keyboard keys call different motion functions. 键盘按键调用不同的运动功能
|
||||
while 1:
|
||||
try:
|
||||
# print("\r current coords: %s" % record_coords, end="")
|
||||
|
|
|
|||
|
|
@ -2,28 +2,29 @@
|
|||
#include <image_transport/image_transport.h>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <sstream> // for converting the command line parameter to integer
|
||||
#include <sstream> // for converting the command line parameter to integer,用于将命令行参数转换为整数
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
// Check if video source has been passed as a parameter
|
||||
// Check if video source has been passed as a parameter,检查视频源是否已作为参数传递
|
||||
if (argv[1] == NULL)
|
||||
{
|
||||
ROS_INFO("argv[1]=NULL\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
ros::init(argc, argv, "image_publisher"); // Initialize node
|
||||
ros::init(argc, argv, "image_publisher"); // Initialize node,初始化节点
|
||||
ros::NodeHandle nh;
|
||||
image_transport::ImageTransport it(nh);
|
||||
image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic
|
||||
image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic，发布话题
|
||||
|
||||
ros::Rate loop_rate(200); // refresh Hz.
|
||||
|
||||
// Convert the passed as command line parameter index for the video device to an integer
|
||||
// Convert the passed as command line parameter index for the video device to an integer，
|
||||
// 将作为命令行参数传递的视频设备索引转换为整数
|
||||
std::istringstream video_sourceCmd(argv[1]);
|
||||
int video_source;
|
||||
// Check if it is indeed a number
|
||||
// Check if it is indeed a number，检查它是否确实是一个数字
|
||||
if (!(video_sourceCmd >> video_source))
|
||||
{
|
||||
ROS_INFO("video_sourceCmd is %d\n", video_source);
|
||||
|
|
@ -31,7 +32,7 @@ int main(int argc, char **argv)
|
|||
}
|
||||
|
||||
cv::VideoCapture cap(video_source);
|
||||
// Check if video device can be opened with the given index
|
||||
// Check if video device can be opened with the given index，检查是否可以使用给定的索引打开视频设备
|
||||
if (!cap.isOpened())
|
||||
{
|
||||
ROS_INFO("can not opencv video device\n");
|
||||
|
|
@ -44,7 +45,7 @@ int main(int argc, char **argv)
|
|||
{
|
||||
cap >> frame;
|
||||
// cv::imshow("veiwer", frame);
|
||||
// Check if grabbed frame is actually full with some content
|
||||
// Check if grabbed frame is actually full with some content，检查抓取的帧是否实际上充满了一些内容
|
||||
if (!frame.empty())
|
||||
{
|
||||
msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
|
||||
|
|
|
|||
|
|
@ -1,11 +1,14 @@
|
|||
<launch>
|
||||
|
||||
<!-- By default, we do not start a database (it can be large) -->
|
||||
<!-- 默认情况下，我们不启动数据库（它可能很大） -->
|
||||
<arg name="db" default="false" />
|
||||
<!-- Allow user to specify database location -->
|
||||
<!-- 允许用户指定数据库位置 -->
|
||||
<arg name="db_path" default="$(find mycobot_280_moveit)/default_warehouse_mongo_db" />
|
||||
|
||||
<!-- By default, we are not in debug mode -->
|
||||
<!-- 默认情况下，我们不处于调试模式 -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!--
|
||||
|
|
@ -19,6 +22,7 @@
|
|||
<arg name="use_gui" default="false" />
|
||||
|
||||
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
|
||||
<!-- 加载参数服务器上的URDF、SRDF等.yaml配置文件-->
|
||||
<include file="$(find mycobot_280_moveit)/launch/planning_context.launch">
|
||||
<arg name="load_robot_description" value="true"/>
|
||||
</include>
|
||||
|
|
@ -27,15 +31,18 @@
|
|||
|
||||
|
||||
<!-- We do not have a robot connected, so publish fake joint states -->
|
||||
<!-- 我们没有连接机器人，所以发布假关节状态 -->
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
|
||||
<param name="use_gui" value="$(arg use_gui)"/>
|
||||
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- Given the published joint states, publish tf for the robot links -->
|
||||
<!-- 给定已发布的关节状态，为机器人链接发布 tf -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
|
||||
|
||||
<!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
|
||||
<!-- 运行主MoveIt！ 没有轨迹执行的可执行文件（我们默认没有配置控制器）-->
|
||||
<include file="$(find mycobot_280_moveit)/launch/move_group.launch">
|
||||
<arg name="allow_trajectory_execution" value="true"/>
|
||||
<arg name="fake_execution" value="true"/>
|
||||
|
|
@ -44,12 +51,14 @@
|
|||
</include>
|
||||
|
||||
<!-- Run Rviz and load the default config to see the state of the move_group node -->
|
||||
<!-- 运行 Rviz 并加载默认配置以查看 move_group 节点的状态 -->
|
||||
<include file="$(find mycobot_280_moveit)/launch/moveit_rviz.launch">
|
||||
<arg name="config" value="true"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
</include>
|
||||
|
||||
<!-- If database loading was enabled, start mongodb as well -->
|
||||
<!-- 如果启用了数据库加载，也启动 mongodb -->
|
||||
<include file="$(find mycobot_280_moveit)/launch/default_warehouse_db.launch" if="$(arg db)">
|
||||
<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
|
||||
</include>
|
||||
|
|
|
|||
|
|
@ -1,11 +1,13 @@
|
|||
<launch>
|
||||
|
||||
<!-- By default, we do not start a database (it can be large) -->
|
||||
<!-- 默认情况下，我们不启动数据库（它可能很大） -->
|
||||
<arg name="db" default="false" />
|
||||
<!-- Allow user to specify database location -->
|
||||
<!-- 允许用户指定数据库位置 -->
|
||||
<arg name="db_path" default="$(find mycobot_280_moveit)/default_warehouse_mongo_db" />
|
||||
|
||||
<!-- By default, we are not in debug mode -->
|
||||
<!-- By default, we are not in debug mode --> <!-- 默认情况下，我们不处于调试模式 -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!--
|
||||
|
|
@ -19,6 +21,7 @@
|
|||
<arg name="use_gui" default="false" />
|
||||
|
||||
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
|
||||
<!-- 加载参数服务器上的URDF、SRDF等.yaml配置文件-->
|
||||
<include file="$(find mycobot_280_moveit)/launch/planning_context.launch">
|
||||
<arg name="load_robot_description" value="true"/>
|
||||
</include>
|
||||
|
|
@ -27,15 +30,18 @@
|
|||
|
||||
|
||||
<!-- We do not have a robot connected, so publish fake joint states -->
|
||||
<!-- 我们没有连接机器人，所以发布假关节状态 -->
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
|
||||
<param name="use_gui" value="$(arg use_gui)"/>
|
||||
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- Given the published joint states, publish tf for the robot links -->
|
||||
<!-- 给定已发布的关节状态，为机器人链接发布 tf -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
|
||||
|
||||
<!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
|
||||
<!-- 运行主MoveIt！ 没有轨迹执行的可执行文件（我们默认没有配置控制器）-->
|
||||
<include file="$(find mycobot_280_moveit)/launch/move_group.launch">
|
||||
<arg name="allow_trajectory_execution" value="true"/>
|
||||
<arg name="fake_execution" value="true"/>
|
||||
|
|
@ -44,12 +50,14 @@
|
|||
</include>
|
||||
|
||||
<!-- Run Rviz and load the default config to see the state of the move_group node -->
|
||||
<!-- 运行 Rviz 并加载默认配置以查看 move_group 节点的状态 -->
|
||||
<include file="$(find mycobot_280_moveit)/launch/moveit_rviz.launch">
|
||||
<arg name="config" value="true"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
</include>
|
||||
|
||||
<!-- If database loading was enabled, start mongodb as well -->
|
||||
<!-- 如果启用了数据库加载，也启动 mongodb -->
|
||||
<include file="$(find mycobot_280_moveit)/launch/default_warehouse_db.launch" if="$(arg db)">
|
||||
<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
|
||||
</include>
|
||||
|
|
|
|||
|
|
@ -12,40 +12,47 @@ from tf.transformations import euler_from_quaternion, quaternion_from_euler
|
|||
|
||||
class MoveItPlanningDemo:
|
||||
def __init__(self):
|
||||
# 初始化move_group的API
|
||||
# API to initialize move_group,初始化move_group的API
|
||||
moveit_commander.roscpp_initialize(sys.argv)
|
||||
|
||||
# 初始化ROS节点
|
||||
# Initialize the ROS node,初始化ROS节点
|
||||
rospy.init_node("moveit_ik_demo")
|
||||
|
||||
# Initialize the scene object to monitor changes in the external environment
|
||||
# 初始化场景对象，用来监听外部环境的变化
|
||||
self.scene = moveit_commander.PlanningSceneInterface()
|
||||
rospy.sleep(1)
|
||||
|
||||
# Initialize self.arm group in the robotic arm that needs to be controlled by move group
|
||||
# 初始化需要使用move group控制的机械臂中的self.arm group
|
||||
self.arm = moveit_commander.MoveGroupCommander("arm_group")
|
||||
|
||||
# 获取终端link的名称
|
||||
# Get the name of the terminal link,获取终端link的名称
|
||||
self.end_effector_link = self.arm.get_end_effector_link()
|
||||
|
||||
# Set the reference coordinate system used for the target position
|
||||
# 设置目标位置所使用的参考坐标系
|
||||
self.reference_frame = "joint1"
|
||||
self.arm.set_pose_reference_frame(self.reference_frame)
|
||||
|
||||
# 当运动规划失败后，允许重新规划
|
||||
# Allow replanning when motion planning fails,当运动规划失败后，允许重新规划
|
||||
self.arm.allow_replanning(True)
|
||||
|
||||
# Set the allowable error of position (unit: meter) and attitude (unit: radian)
|
||||
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
|
||||
self.arm.set_goal_position_tolerance(0.01)
|
||||
self.arm.set_goal_orientation_tolerance(0.05)
|
||||
|
||||
def moving(self):
|
||||
# # 控制机械臂先回到初始化位置
|
||||
# # Control the robotic arm to return to the initialization position first
|
||||
# 控制机械臂先回到初始化位置
|
||||
self.arm.set_named_target("init_pose")
|
||||
self.arm.go()
|
||||
rospy.sleep(2)
|
||||
|
||||
# Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates,
|
||||
# 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
|
||||
# Pose is described by quaternion, based on base_link coordinate system
|
||||
# 姿态使用四元数描述，基于base_link坐标系
|
||||
target_pose = PoseStamped()
|
||||
target_pose.header.frame_id = self.reference_frame
|
||||
|
|
@ -58,19 +65,23 @@ class MoveItPlanningDemo:
|
|||
target_pose.pose.orientation.z = 0.0
|
||||
target_pose.pose.orientation.w = 0.014
|
||||
|
||||
# Set the current state of the robot arm as the initial state of motion
|
||||
# 设置机器臂当前的状态作为运动初始状态
|
||||
self.arm.set_start_state_to_current_state()
|
||||
|
||||
# Set the target pose of the terminal motion of the robotic arm
|
||||
# 设置机械臂终端运动的目标位姿
|
||||
self.arm.set_pose_target(target_pose, self.end_effector_link)
|
||||
|
||||
# 规划运动路径
|
||||
# Plan the movement path,规划运动路径
|
||||
traj = self.arm.plan()
|
||||
|
||||
# Control the motion of the robotic arm according to the planned motion path
|
||||
# 按照规划的运动路径控制机械臂运动
|
||||
self.arm.execute(traj)
|
||||
rospy.sleep(1)
|
||||
|
||||
# Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy
|
||||
# 控制机械臂终端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy
|
||||
self.arm.shift_pose_target(1, 0.12, self.end_effector_link)
|
||||
self.arm.go()
|
||||
|
|
@ -80,6 +91,7 @@ class MoveItPlanningDemo:
|
|||
self.arm.go()
|
||||
rospy.sleep(1)
|
||||
|
||||
# Control the terminal of the robotic arm to rotate 90 degrees in the opposite direction. 0,1,2,3,4,5 represent xyzrpy
|
||||
# 控制机械臂终端反向旋转90度 0,1,2,3,4,5 代表xyzrpy
|
||||
# self.arm.shift_pose_target(3, -1.57, end_effector_link)
|
||||
# self.arm.go()
|
||||
|
|
@ -88,10 +100,10 @@ class MoveItPlanningDemo:
|
|||
def run(self):
|
||||
self.scene.remove_world_object("suit")
|
||||
|
||||
# 没有障碍物运行一次
|
||||
# Run once without obstacles,没有障碍物运行一次
|
||||
self.moving()
|
||||
|
||||
# 添加环境
|
||||
# Add environment,添加环境
|
||||
quat = quaternion_from_euler(3.1415, 0, -1.57)
|
||||
|
||||
suit_post = PoseStamped()
|
||||
|
|
@ -112,10 +124,10 @@ class MoveItPlanningDemo:
|
|||
self.scene.add_mesh("suit", suit_post, suit_path)
|
||||
rospy.sleep(2)
|
||||
|
||||
# 有环境影响后在运行一次
|
||||
# Run it again if there is an environmental impact,有环境影响后再运行一次
|
||||
self.moving()
|
||||
|
||||
# 关闭并退出moveit
|
||||
# close and exit moveit，关闭并退出moveit
|
||||
moveit_commander.roscpp_shutdown()
|
||||
moveit_commander.os._exit(0)
|
||||
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@ def listener():
|
|||
rospy.Subscriber("joint_states", JointState, callback)
|
||||
|
||||
# spin() simply keeps python from exiting until this node is stopped
|
||||
# spin() 只是阻止 python 退出，直到该节点停止
|
||||
rospy.spin()
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,15 +1,17 @@
|
|||
<launch>
|
||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot_with_marker.rviz" />
|
||||
<arg name="gui" default="true" />
|
||||
|
||||
<arg name="num" default="0" />
|
||||
|
||||
<!-- Add model control,添加模型控制 -->
|
||||
<include file="$(find mycobot_280jn)/launch/slider_control.launch">
|
||||
<arg name="model" value="$(arg model)" />
|
||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
</include>
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="mycobot_280jn" type="opencv_camera" args="$(arg num)"/>
|
||||
<node name="detect_marker" pkg="mycobot_280jn" type="detect_marker.py" />
|
||||
<node name="following_marker" pkg="mycobot_280jn" type="following_marker.py" />
|
||||
|
|
|
|||
|
|
@ -1,7 +1,8 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyTHS1" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot_with_marker.rviz" />
|
||||
<arg name="gui" default="false" />
|
||||
|
|
@ -10,17 +11,17 @@
|
|||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF -->
|
||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<!-- Show in Rviz -->
|
||||
<!-- Show in Rviz，显示在Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<!-- mycobot-topics -->
|
||||
<!-- mycobot-topics， mycobot-话题-->
|
||||
<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 -->
|
||||
<!-- listen and pub the real angles ，监听并发布真实的角度-->
|
||||
<node name="real_listener" pkg="mycobot_280jn" type="listen_real_of_topic.py" />
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="mycobot_280jn" type="opencv_camera" args="$(arg num)"/>
|
||||
|
|
|
|||
|
|
@ -1,11 +1,13 @@
|
|||
<launch>
|
||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot.rviz" />
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- mycobot-topics，mycobot-话题 -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
|
||||
<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
|
||||
</node>
|
||||
<!-- Show in Rviz，显示在Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,22 +1,24 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyTHS1" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot.rviz" />
|
||||
<arg name="gui" default="false" />
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF -->
|
||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<!-- Show in Rviz -->
|
||||
<!-- 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">
|
||||
<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_280jn" type="listen_real.py" />
|
||||
<node name="simple_gui" pkg="mycobot_280jn" type="simple_gui.py" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,14 +1,14 @@
|
|||
<launch>
|
||||
<!-- <arg name="port" default="/dev/ttyTHS1" />
|
||||
<arg name="baud" default="1000000" /> -->
|
||||
|
||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot.rviz" />
|
||||
<arg name="gui" default="true" />
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF -->
|
||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
|
||||
<!-- <param name="use_gui" value="$(arg gui)" /> -->
|
||||
|
|
@ -19,6 +19,6 @@
|
|||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
</node> -->
|
||||
<!-- Show in Rviz -->
|
||||
<!-- Show in Rviz ，显示在Rviz-->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,21 +1,23 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyTHS1" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot.rviz" />
|
||||
<arg name="gui" default="false" />
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF -->
|
||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<!-- Show in Rviz -->
|
||||
<!-- 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">
|
||||
<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_280jn" type="listen_real.py" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,16 +1,17 @@
|
|||
<launch>
|
||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
|
||||
<arg name="gui" default="true" />
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF -->
|
||||
<!-- Combinejoin values to TF ,将值合并到TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
|
||||
<param name="use_gui" value="$(arg gui)" />
|
||||
<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
|
||||
</node>
|
||||
<!-- Show in Rviz -->
|
||||
<!-- Show in Rviz ，显示在Rviz-->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ class ImageConverter:
|
|||
)
|
||||
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
|
||||
self.camera_matrix = None
|
||||
# subscriber, listen wether has img come in.
|
||||
# subscriber, listen wether has img come in. 订阅者，监听是否有img
|
||||
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
|
||||
|
||||
def callback(self, data):
|
||||
|
|
@ -37,7 +37,7 @@ class ImageConverter:
|
|||
pose to transforming.
|
||||
"""
|
||||
try:
|
||||
# trans `rgb` to `gbr` for opencv.
|
||||
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
|
||||
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
|
|
@ -45,7 +45,7 @@ class ImageConverter:
|
|||
focal_length = size[1]
|
||||
center = [size[1] / 2, size[0] / 2]
|
||||
if self.camera_matrix is None:
|
||||
# calc the camera matrix, if don't have.
|
||||
# calc the camera matrix, if don't have.如果没有，则计算相机矩阵
|
||||
self.camera_matrix = np.array(
|
||||
[
|
||||
[focal_length, 0, center[0]],
|
||||
|
|
@ -55,18 +55,18 @@ class ImageConverter:
|
|||
dtype=np.float32,
|
||||
)
|
||||
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
|
||||
# detect aruco marker.
|
||||
# detect aruco marker.检测 aruco 标记
|
||||
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
|
||||
corners, ids = ret[0], ret[1]
|
||||
# process marker data.
|
||||
# process marker data.处理标记数据
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
# print('corners:', corners, 'ids:', ids)
|
||||
|
||||
# detect marker pose.
|
||||
# detect marker pose. 检测marker位姿。
|
||||
# argument:
|
||||
# marker corners
|
||||
# marker size (meter)
|
||||
# marker corners,标记角
|
||||
# marker size (meter),标记尺寸（米）
|
||||
ret = cv.aruco.estimatePoseSingleMarkers(
|
||||
corners, 0.05, self.camera_matrix, self.dist_coeffs
|
||||
)
|
||||
|
|
@ -75,7 +75,7 @@ class ImageConverter:
|
|||
|
||||
print("rvec:", rvec, "tvec:", tvec)
|
||||
|
||||
# just select first one detected marker.
|
||||
# just select first one detected marker.只需选择第一个检测到的标记。
|
||||
for i in range(rvec.shape[0]):
|
||||
cv.aruco.drawDetectedMarkers(cv_image, corners)
|
||||
cv.aruco.drawAxis(
|
||||
|
|
@ -90,14 +90,14 @@ class ImageConverter:
|
|||
xyz = tvec[0, 0, :]
|
||||
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
|
||||
|
||||
# get quaternion for ros.
|
||||
# get quaternion for ros. 为ros获取四元数
|
||||
euler = rvec[0, 0, :]
|
||||
tf_change = tf.transformations.quaternion_from_euler(
|
||||
euler[0], euler[1], euler[2]
|
||||
)
|
||||
print("tf_change:", tf_change)
|
||||
|
||||
# trans pose according [joint1]
|
||||
# trans pose according [joint1]，根据 [joint1] 变换姿势
|
||||
self.br.sendTransform(
|
||||
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
|
||||
)
|
||||
|
|
|
|||
|
|
@ -5,18 +5,19 @@ from visualization_msgs.msg import Marker
|
|||
import time
|
||||
import os
|
||||
|
||||
# 与 mycobot 通信的消息类型
|
||||
# Type of message communicated with mycobot，与 mycobot 通信的消息类型
|
||||
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
|
||||
|
||||
|
||||
rospy.init_node("gipper_subscriber", anonymous=True)
|
||||
|
||||
# Control the topic of mycobot, followed by angle, coordinates, gripper
|
||||
# 控制 mycobot 的 topic，依次是角度、坐标、夹爪
|
||||
angle_pub = rospy.Publisher("mycobot/angles_goal",
|
||||
MycobotSetAngles, queue_size=5)
|
||||
coord_pub = rospy.Publisher("mycobot/coords_goal",
|
||||
MycobotSetCoords, queue_size=5)
|
||||
# Judging equipment: ttyUSB* is M5；ttyACM* is wio
|
||||
# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
|
||||
robot = os.popen("ls /dev/ttyUSB*").readline()
|
||||
|
||||
if "dev" in robot:
|
||||
|
|
@ -27,27 +28,28 @@ else:
|
|||
pump_pub = rospy.Publisher("mycobot/pump_status",
|
||||
MycobotPumpStatus, queue_size=5)
|
||||
|
||||
# 实例化消息对象
|
||||
# instantiate the message object，实例化消息对象
|
||||
angles = MycobotSetAngles()
|
||||
coords = MycobotSetCoords()
|
||||
pump = MycobotPumpStatus()
|
||||
|
||||
# 与 mycobot 真实位置的偏差值
|
||||
# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值
|
||||
x_offset = -20
|
||||
y_offset = 20
|
||||
z_offset = 110
|
||||
|
||||
# With this variable limit, the fetching behavior is only done once
|
||||
# 通过该变量限制，抓取行为只做一次
|
||||
flag = False
|
||||
|
||||
# 为了后面比较二维码是否移动
|
||||
# In order to compare whether the QR code moves later,为了后面比较二维码是否移动
|
||||
temp_x = temp_y = temp_z = 0.0
|
||||
|
||||
temp_time = time.time()
|
||||
|
||||
|
||||
def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
||||
"""发布坐标"""
|
||||
"""Post coordinates,发布坐标"""
|
||||
coords.x = x
|
||||
coords.y = y
|
||||
coords.z = z
|
||||
|
|
@ -61,7 +63,7 @@ def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
|||
|
||||
|
||||
def pub_angles(a, b, c, d, e, f, sp):
|
||||
"""发布角度"""
|
||||
"""Publishing angle,发布角度"""
|
||||
angles.joint_1 = float(a)
|
||||
angles.joint_2 = float(b)
|
||||
angles.joint_3 = float(c)
|
||||
|
|
@ -73,7 +75,7 @@ def pub_angles(a, b, c, d, e, f, sp):
|
|||
|
||||
|
||||
def pub_pump(flag, Pin):
|
||||
"""发布夹爪状态"""
|
||||
"""Publish gripper status,发布夹爪状态"""
|
||||
pump.Status = flag
|
||||
pump.Pin1 = Pin[0]
|
||||
pump.Pin2 = Pin[1]
|
||||
|
|
@ -81,6 +83,7 @@ def pub_pump(flag, Pin):
|
|||
|
||||
|
||||
def target_is_moving(x, y, z):
|
||||
"""Determine whether the target moves"""
|
||||
"""判断目标是否移动"""
|
||||
count = 0
|
||||
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
|
||||
|
|
@ -94,18 +97,19 @@ def target_is_moving(x, y, z):
|
|||
|
||||
|
||||
def grippercallback(data):
|
||||
"""回调函数"""
|
||||
"""callback function,回调函数"""
|
||||
global flag, temp_x, temp_y, temp_z
|
||||
# rospy.loginfo('gripper_subscriber get date :%s', data)
|
||||
if flag:
|
||||
return
|
||||
|
||||
# 解析出坐标值
|
||||
# Parse out the coordinate value,解析出坐标值
|
||||
# pump length: 88mm
|
||||
x = float(format(data.pose.position.x * 1000, ".2f"))
|
||||
y = float(format(data.pose.position.y * 1000, ".2f"))
|
||||
z = float(format(data.pose.position.z * 1000, ".2f"))
|
||||
|
||||
# When the running time is less than 30s, or the target position is still changing, perform tracking behavior
|
||||
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
|
||||
if (
|
||||
time.time() - temp_time < 30
|
||||
|
|
@ -120,7 +124,7 @@ def grippercallback(data):
|
|||
|
||||
temp_x, temp_y, temp_z = x, y, z
|
||||
return
|
||||
else: # 表示目标处于静止状态，可以尝试抓取
|
||||
else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
|
||||
|
||||
print(x, y, z)
|
||||
|
||||
|
|
@ -178,7 +182,7 @@ def main():
|
|||
pub_pump(False, Pin)
|
||||
# time.sleep(2.5)
|
||||
|
||||
# mark 信息的订阅者
|
||||
# subscribers to mark information，mark 信息的订阅者
|
||||
rospy.Subscriber("visualization_marker", Marker,
|
||||
grippercallback, queue_size=1)
|
||||
|
||||
|
|
|
|||
|
|
@ -79,7 +79,7 @@ def talker():
|
|||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
|
||||
# marker position initial
|
||||
# marker position initial.标记位置初始
|
||||
# print(coords)
|
||||
if not coords:
|
||||
coords = [0, 0, 0, 0, 0, 0]
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@ def talker():
|
|||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
|
||||
# marker position initial
|
||||
# marker position initial,标记位置初始化
|
||||
marker_.pose.position.x = trans[0]
|
||||
marker_.pose.position.y = trans[1]
|
||||
marker_.pose.position.z = trans[2]
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ def talker():
|
|||
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||
rate = rospy.Rate(30) # 30hz
|
||||
|
||||
# pub joint state
|
||||
# pub joint state，发布关节状态
|
||||
joint_state_send = JointState()
|
||||
joint_state_send.header = Header()
|
||||
|
||||
|
|
@ -30,14 +30,14 @@ def talker():
|
|||
joint_state_send.velocity = [0]
|
||||
joint_state_send.effort = []
|
||||
|
||||
# waiting util server `get_joint_angles` enable.
|
||||
# waiting util server `get_joint_angles` enable.等待'get_joint_angles'服务启用
|
||||
rospy.loginfo("wait service")
|
||||
rospy.wait_for_service("get_joint_angles")
|
||||
func = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
||||
|
||||
rospy.loginfo("start loop ...")
|
||||
while not rospy.is_shutdown():
|
||||
# get real angles from server.
|
||||
# get real angles from server.从服务器获得真实的角度。
|
||||
res = func()
|
||||
if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
|
||||
continue
|
||||
|
|
@ -51,7 +51,7 @@ def talker():
|
|||
]
|
||||
rospy.loginfo("res: {}".format(radians_list))
|
||||
|
||||
# publish angles.
|
||||
# publish angles.发布角度
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
joint_state_send.position = radians_list
|
||||
pub.publish(joint_state_send)
|
||||
|
|
|
|||
|
|
@ -13,9 +13,9 @@ class Listener(object):
|
|||
|
||||
rospy.loginfo("start ...")
|
||||
rospy.init_node("real_listener_1", anonymous=True)
|
||||
# init publisher.
|
||||
# init publisher.初始化发布者
|
||||
self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||
# init subscriber.
|
||||
# init subscriber.初始化订阅者
|
||||
self.sub = rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.callback)
|
||||
rospy.spin()
|
||||
|
||||
|
|
@ -25,7 +25,7 @@ class Listener(object):
|
|||
Args:
|
||||
data (MycobotAngles): callback argument.
|
||||
"""
|
||||
# ini publisher object.
|
||||
# ini publisher object. 初始化发布者对象
|
||||
joint_state_send = JointState()
|
||||
joint_state_send.header = Header()
|
||||
|
||||
|
|
@ -41,7 +41,7 @@ class Listener(object):
|
|||
joint_state_send.effort = []
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
|
||||
# process callback data.
|
||||
# process callback data. 处理回调数据。
|
||||
radians_list = [
|
||||
data.joint_1 * (math.pi / 180),
|
||||
data.joint_2 * (math.pi / 180),
|
||||
|
|
|
|||
|
|
@ -10,47 +10,48 @@ from rospy import ServiceException
|
|||
class Window:
|
||||
def __init__(self, handle):
|
||||
self.win = handle
|
||||
self.win.resizable(0, 0) # 固定窗口大小
|
||||
self.win.resizable(0, 0) # fixed window size，固定窗口大小
|
||||
|
||||
self.model = 0
|
||||
self.speed = rospy.get_param("~speed", 50)
|
||||
|
||||
# 设置默认速度
|
||||
# set default speed，设置默认速度
|
||||
self.speed_d = tk.StringVar()
|
||||
self.speed_d.set(str(self.speed))
|
||||
# print(self.speed)
|
||||
self.connect_ser()
|
||||
|
||||
# 获取机械臂数据
|
||||
# Get the data of the robotic arm，获取机械臂数据
|
||||
self.record_coords = [0, 0, 0, 0, 0, 0, self.speed, self.model]
|
||||
self.res_angles = [0, 0, 0, 0, 0, 0, self.speed, self.model]
|
||||
self.get_date()
|
||||
|
||||
# get screen width and height
|
||||
# get screen width and height.获取屏幕宽度和高度
|
||||
self.ws = self.win.winfo_screenwidth() # width of the screen
|
||||
self.hs = self.win.winfo_screenheight() # height of the screen
|
||||
# calculate x and y coordinates for the Tk root window
|
||||
# 计算 Tk 根窗口的 x 和 y 坐标
|
||||
x = (self.ws / 2) - 190
|
||||
y = (self.hs / 2) - 250
|
||||
self.win.geometry("430x370+{}+{}".format(x, y))
|
||||
# 布局
|
||||
# layout,布局
|
||||
self.set_layout()
|
||||
# 输入部分
|
||||
# input section,输入部分
|
||||
self.need_input()
|
||||
# 展示部分
|
||||
# Show part,展示部分
|
||||
self.show_init()
|
||||
|
||||
# joint 设置按钮
|
||||
# Set the joint buttons 设置joint按钮
|
||||
tk.Button(self.frmLT, text="设置", width=5, command=self.get_joint_input).grid(
|
||||
row=6, column=1, sticky="w", padx=3, pady=2
|
||||
)
|
||||
|
||||
# coordination 设置按钮
|
||||
# coordination settings button,coordination 设置按钮
|
||||
tk.Button(self.frmRT, text="设置", width=5, command=self.get_coord_input).grid(
|
||||
row=6, column=1, sticky="w", padx=3, pady=2
|
||||
)
|
||||
|
||||
# 夹爪开关按钮
|
||||
# Gripper switch button,夹爪开关按钮
|
||||
tk.Button(self.frmLB, text="夹爪(开)", command=self.gripper_open, width=5).grid(
|
||||
row=1, column=0, sticky="w", padx=3, pady=50
|
||||
)
|
||||
|
|
@ -91,22 +92,22 @@ class Window:
|
|||
self.frmRT.grid(row=0, column=1, padx=2, pady=3)
|
||||
|
||||
def need_input(self):
|
||||
# 输入提示
|
||||
# input hint,输入提示
|
||||
tk.Label(self.frmLT, text="Joint 1 ").grid(row=0)
|
||||
tk.Label(self.frmLT, text="Joint 2 ").grid(row=1) # 第二行
|
||||
tk.Label(self.frmLT, text="Joint 2 ").grid(row=1) # the second row,第二行
|
||||
tk.Label(self.frmLT, text="Joint 3 ").grid(row=2)
|
||||
tk.Label(self.frmLT, text="Joint 4 ").grid(row=3)
|
||||
tk.Label(self.frmLT, text="Joint 5 ").grid(row=4)
|
||||
tk.Label(self.frmLT, text="Joint 6 ").grid(row=5)
|
||||
|
||||
tk.Label(self.frmRT, text=" x ").grid(row=0)
|
||||
tk.Label(self.frmRT, text=" y ").grid(row=1) # 第二行
|
||||
tk.Label(self.frmRT, text=" y ").grid(row=1) # the second row,第二行
|
||||
tk.Label(self.frmRT, text=" z ").grid(row=2)
|
||||
tk.Label(self.frmRT, text=" rx ").grid(row=3)
|
||||
tk.Label(self.frmRT, text=" ry ").grid(row=4)
|
||||
tk.Label(self.frmRT, text=" rz ").grid(row=5)
|
||||
|
||||
# 设置输入框的默认值
|
||||
# Set the default value of the input box,设置输入框的默认值
|
||||
self.j1_default = tk.StringVar()
|
||||
self.j1_default.set(self.res_angles[0])
|
||||
self.j2_default = tk.StringVar()
|
||||
|
|
@ -133,7 +134,7 @@ class Window:
|
|||
self.rz_default = tk.StringVar()
|
||||
self.rz_default.set(self.record_coords[5])
|
||||
|
||||
# joint 输入框
|
||||
# joint input box,joint 输入框
|
||||
self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default)
|
||||
self.J_1.grid(row=0, column=1, pady=3)
|
||||
self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default)
|
||||
|
|
@ -147,7 +148,7 @@ class Window:
|
|||
self.J_6 = tk.Entry(self.frmLT, textvariable=self.j6_default)
|
||||
self.J_6.grid(row=5, column=1, pady=3)
|
||||
|
||||
# coord 输入框
|
||||
# coord input box,coord 输入框
|
||||
self.x = tk.Entry(self.frmRT, textvariable=self.x_default)
|
||||
self.x.grid(row=0, column=1, pady=3, padx=0)
|
||||
self.y = tk.Entry(self.frmRT, textvariable=self.y_default)
|
||||
|
|
@ -161,11 +162,11 @@ class Window:
|
|||
self.rz = tk.Entry(self.frmRT, textvariable=self.rz_default)
|
||||
self.rz.grid(row=5, column=1, pady=3)
|
||||
|
||||
# 所有输入框，用于拿输入的数据
|
||||
# All input boxes, used to get the input data,所有输入框，用于拿输入的数据
|
||||
self.all_j = [self.J_1, self.J_2, self.J_3, self.J_4, self.J_5, self.J_6]
|
||||
self.all_c = [self.x, self.y, self.z, self.rx, self.ry, self.rz]
|
||||
|
||||
# 速度输入框
|
||||
# speed input box,速度输入框
|
||||
tk.Label(
|
||||
self.frmLB,
|
||||
text="speed",
|
||||
|
|
@ -174,9 +175,9 @@ class Window:
|
|||
self.get_speed.grid(row=0, column=1)
|
||||
|
||||
def show_init(self):
|
||||
# 显示
|
||||
# show,显示
|
||||
tk.Label(self.frmLC, text="Joint 1 ").grid(row=0)
|
||||
tk.Label(self.frmLC, text="Joint 2 ").grid(row=1) # 第二行
|
||||
tk.Label(self.frmLC, text="Joint 2 ").grid(row=1) # the second row,第二行
|
||||
tk.Label(self.frmLC, text="Joint 3 ").grid(row=2)
|
||||
tk.Label(self.frmLC, text="Joint 4 ").grid(row=3)
|
||||
tk.Label(self.frmLC, text="Joint 5 ").grid(row=4)
|
||||
|
|
@ -184,7 +185,7 @@ class Window:
|
|||
|
||||
# get数据
|
||||
|
||||
# ，展示出来
|
||||
# show,展示出来
|
||||
self.cont_1 = tk.StringVar(self.frmLC)
|
||||
self.cont_1.set(str(self.res_angles[0]) + "°")
|
||||
self.cont_2 = tk.StringVar(self.frmLC)
|
||||
|
|
@ -267,9 +268,9 @@ class Window:
|
|||
self.show_j6,
|
||||
]
|
||||
|
||||
# 显示
|
||||
# show,显示
|
||||
tk.Label(self.frmLC, text=" x ").grid(row=0, column=3)
|
||||
tk.Label(self.frmLC, text=" y ").grid(row=1, column=3) # 第二行
|
||||
tk.Label(self.frmLC, text=" y ").grid(row=1, column=3)
|
||||
tk.Label(self.frmLC, text=" z ").grid(row=2, column=3)
|
||||
tk.Label(self.frmLC, text=" rx ").grid(row=3, column=3)
|
||||
tk.Label(self.frmLC, text=" ry ").grid(row=4, column=3)
|
||||
|
|
@ -347,7 +348,7 @@ class Window:
|
|||
bg="white",
|
||||
).grid(row=5, column=4, padx=5, pady=5)
|
||||
|
||||
# mm 单位展示
|
||||
# mm， Unit show，单位展示
|
||||
self.unit = tk.StringVar()
|
||||
self.unit.set("mm")
|
||||
for i in range(6):
|
||||
|
|
@ -359,6 +360,7 @@ class Window:
|
|||
try:
|
||||
self.switch_gripper(True)
|
||||
except ServiceException:
|
||||
# Probably because the method has no return value, the service throws an unhandled error
|
||||
# 可能由于该方法没有返回值，服务抛出无法处理的错误
|
||||
pass
|
||||
|
||||
|
|
@ -369,6 +371,7 @@ class Window:
|
|||
pass
|
||||
|
||||
def get_coord_input(self):
|
||||
# Get the data input by coord and send it to the robotic arm
|
||||
# 获取 coord 输入的数据，发送给机械臂
|
||||
c_value = []
|
||||
for i in self.all_c:
|
||||
|
|
@ -387,6 +390,7 @@ class Window:
|
|||
self.show_j_date(c_value[:-2], "coord")
|
||||
|
||||
def get_joint_input(self):
|
||||
# Get the data input by the joint and send it to the robotic arm
|
||||
# 获取joint输入的数据，发送给机械臂
|
||||
j_value = []
|
||||
for i in self.all_j:
|
||||
|
|
@ -405,7 +409,7 @@ class Window:
|
|||
# return j_value,c_value,speed
|
||||
|
||||
def get_date(self):
|
||||
# 拿机械臂的数据，用于展示
|
||||
# Take the data of the robotic arm for display.拿机械臂的数据，用于展示
|
||||
t = time.time()
|
||||
while time.time() - t < 2:
|
||||
self.res = self.get_coords()
|
||||
|
|
@ -443,7 +447,7 @@ class Window:
|
|||
|
||||
# def send_input(self,dates):
|
||||
def show_j_date(self, date, way=""):
|
||||
# 展示数据
|
||||
# Show data,展示数据
|
||||
if way == "coord":
|
||||
for i, j in zip(date, self.coord_all):
|
||||
# print(i)
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@ def listener():
|
|||
mc = MyCobot(port, baud)
|
||||
|
||||
# spin() simply keeps python from exiting until this node is stopped
|
||||
# spin()只是阻止python退出，直到该节点停止
|
||||
print("spin ...")
|
||||
rospy.spin()
|
||||
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ import time
|
|||
|
||||
import roslib
|
||||
|
||||
|
||||
# Terminal output prompt information. 终端输出提示信息
|
||||
msg = """\
|
||||
Mycobot Teleop Keyboard Controller
|
||||
---------------------------
|
||||
|
|
@ -97,6 +97,7 @@ def teleop_keyboard():
|
|||
try:
|
||||
print(msg)
|
||||
print(vels(speed, change_percent))
|
||||
# Keyboard keys call different motion functions. 键盘按键调用不同的运动功能
|
||||
while 1:
|
||||
try:
|
||||
# print("\r current coords: %s" % record_coords, end="")
|
||||
|
|
|
|||
|
|
@ -2,28 +2,29 @@
|
|||
#include <image_transport/image_transport.h>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <sstream> // for converting the command line parameter to integer
|
||||
#include <sstream> // for converting the command line parameter to integer,用于将命令行参数转换为整数
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
// Check if video source has been passed as a parameter
|
||||
// Check if video source has been passed as a parameter,检查视频源是否已作为参数传递
|
||||
if (argv[1] == NULL)
|
||||
{
|
||||
ROS_INFO("argv[1]=NULL\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
ros::init(argc, argv, "image_publisher"); // Initialize node
|
||||
ros::init(argc, argv, "image_publisher"); // Initialize node,初始化节点
|
||||
ros::NodeHandle nh;
|
||||
image_transport::ImageTransport it(nh);
|
||||
image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic
|
||||
image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic，发布话题
|
||||
|
||||
ros::Rate loop_rate(200); // refresh Hz.
|
||||
|
||||
// Convert the passed as command line parameter index for the video device to an integer
|
||||
// Convert the passed as command line parameter index for the video device to an integer，
|
||||
// 将作为命令行参数传递的视频设备索引转换为整数
|
||||
std::istringstream video_sourceCmd(argv[1]);
|
||||
int video_source;
|
||||
// Check if it is indeed a number
|
||||
// Check if it is indeed a number，检查它是否确实是一个数字
|
||||
if (!(video_sourceCmd >> video_source))
|
||||
{
|
||||
ROS_INFO("video_sourceCmd is %d\n", video_source);
|
||||
|
|
@ -31,7 +32,7 @@ int main(int argc, char **argv)
|
|||
}
|
||||
|
||||
cv::VideoCapture cap(video_source);
|
||||
// Check if video device can be opened with the given index
|
||||
// Check if video device can be opened with the given index，检查是否可以使用给定的索引打开视频设备
|
||||
if (!cap.isOpened())
|
||||
{
|
||||
ROS_INFO("can not opencv video device\n");
|
||||
|
|
@ -44,7 +45,7 @@ int main(int argc, char **argv)
|
|||
{
|
||||
cap >> frame;
|
||||
// cv::imshow("veiwer", frame);
|
||||
// Check if grabbed frame is actually full with some content
|
||||
// Check if grabbed frame is actually full with some content，检查抓取的帧是否实际上充满了一些内容
|
||||
if (!frame.empty())
|
||||
{
|
||||
msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
|
||||
|
|
|
|||
|
|
@ -1,11 +1,14 @@
|
|||
<launch>
|
||||
|
||||
<!-- By default, we do not start a database (it can be large) -->
|
||||
<!-- 默认情况下，我们不启动数据库（它可能很大） -->
|
||||
<arg name="db" default="false" />
|
||||
<!-- Allow user to specify database location -->
|
||||
<!-- 允许用户指定数据库位置 -->
|
||||
<arg name="db_path" default="$(find mycobot_280jn_moveit)/default_warehouse_mongo_db" />
|
||||
|
||||
<!-- By default, we are not in debug mode -->
|
||||
<!-- 默认情况下，我们不处于调试模式 -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!--
|
||||
|
|
@ -19,6 +22,7 @@
|
|||
<arg name="use_gui" default="false" />
|
||||
|
||||
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
|
||||
<!-- 加载参数服务器上的URDF、SRDF等.yaml配置文件-->
|
||||
<include file="$(find mycobot_280jn_moveit)/launch/planning_context.launch">
|
||||
<arg name="load_robot_description" value="true"/>
|
||||
</include>
|
||||
|
|
@ -27,15 +31,18 @@
|
|||
|
||||
|
||||
<!-- We do not have a robot connected, so publish fake joint states -->
|
||||
<!-- 我们没有连接机器人，所以发布假关节状态 -->
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
|
||||
<param name="use_gui" value="$(arg use_gui)"/>
|
||||
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- Given the published joint states, publish tf for the robot links -->
|
||||
<!-- 给定已发布的关节状态，为机器人链接发布 tf -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
|
||||
|
||||
<!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
|
||||
<!-- 运行主MoveIt！ 没有轨迹执行的可执行文件（我们默认没有配置控制器）-->
|
||||
<include file="$(find mycobot_280jn_moveit)/launch/move_group.launch">
|
||||
<arg name="allow_trajectory_execution" value="true"/>
|
||||
<arg name="fake_execution" value="true"/>
|
||||
|
|
@ -43,13 +50,15 @@
|
|||
<arg name="debug" value="$(arg debug)"/>
|
||||
</include>
|
||||
|
||||
<!-- Run Rviz and load the default config to see the state of the move_group node
|
||||
<!-- Run Rviz and load the default config to see the state of the move_group node -->
|
||||
<!-- 运行 Rviz 并加载默认配置以查看 move_group 节点的状态 -->
|
||||
<include file="$(find mycobot_280jn_moveit)/launch/moveit_rviz.launch">
|
||||
<arg name="config" value="true"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
</include> -->
|
||||
|
||||
</include>
|
||||
|
||||
<!-- If database loading was enabled, start mongodb as well -->
|
||||
<!-- 如果启用了数据库加载，也启动 mongodb -->
|
||||
<include file="$(find mycobot_280jn_moveit)/launch/default_warehouse_db.launch" if="$(arg db)">
|
||||
<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
|
||||
</include>
|
||||
|
|
|
|||
|
|
@ -1,11 +1,13 @@
|
|||
<launch>
|
||||
|
||||
<!-- By default, we do not start a database (it can be large) -->
|
||||
<!-- 默认情况下，我们不启动数据库（它可能很大） -->
|
||||
<arg name="db" default="false" />
|
||||
<!-- Allow user to specify database location -->
|
||||
<!-- 允许用户指定数据库位置 -->
|
||||
<arg name="db_path" default="$(find mycobot_280jn_moveit)/default_warehouse_mongo_db" />
|
||||
|
||||
<!-- By default, we are not in debug mode -->
|
||||
<!-- By default, we are not in debug mode --> <!-- 默认情况下，我们不处于调试模式 -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!--
|
||||
|
|
@ -19,6 +21,7 @@
|
|||
<arg name="use_gui" default="false" />
|
||||
|
||||
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
|
||||
<!-- 加载参数服务器上的URDF、SRDF等.yaml配置文件-->
|
||||
<include file="$(find mycobot_280jn_moveit)/launch/planning_context.launch">
|
||||
<arg name="load_robot_description" value="true"/>
|
||||
</include>
|
||||
|
|
@ -27,15 +30,18 @@
|
|||
|
||||
|
||||
<!-- We do not have a robot connected, so publish fake joint states -->
|
||||
<!-- 我们没有连接机器人，所以发布假关节状态 -->
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
|
||||
<param name="use_gui" value="$(arg use_gui)"/>
|
||||
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- Given the published joint states, publish tf for the robot links -->
|
||||
<!-- 给定已发布的关节状态，为机器人链接发布 tf -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
|
||||
|
||||
<!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
|
||||
<!-- 运行主MoveIt！ 没有轨迹执行的可执行文件（我们默认没有配置控制器）-->
|
||||
<include file="$(find mycobot_280jn_moveit)/launch/move_group.launch">
|
||||
<arg name="allow_trajectory_execution" value="true"/>
|
||||
<arg name="fake_execution" value="true"/>
|
||||
|
|
@ -44,12 +50,14 @@
|
|||
</include>
|
||||
|
||||
<!-- Run Rviz and load the default config to see the state of the move_group node -->
|
||||
<!-- 运行 Rviz 并加载默认配置以查看 move_group 节点的状态 -->
|
||||
<include file="$(find mycobot_280jn_moveit)/launch/moveit_rviz.launch">
|
||||
<arg name="config" value="true"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
</include>
|
||||
|
||||
<!-- If database loading was enabled, start mongodb as well -->
|
||||
<!-- 如果启用了数据库加载，也启动 mongodb -->
|
||||
<include file="$(find mycobot_280jn_moveit)/launch/default_warehouse_db.launch" if="$(arg db)">
|
||||
<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
|
||||
</include>
|
||||
|
|
|
|||
|
|
@ -12,40 +12,47 @@ from tf.transformations import euler_from_quaternion, quaternion_from_euler
|
|||
|
||||
class MoveItPlanningDemo:
|
||||
def __init__(self):
|
||||
# 初始化move_group的API
|
||||
# API to initialize move_group,初始化move_group的API
|
||||
moveit_commander.roscpp_initialize(sys.argv)
|
||||
|
||||
# 初始化ROS节点
|
||||
# Initialize the ROS node,初始化ROS节点
|
||||
rospy.init_node("moveit_ik_demo")
|
||||
|
||||
# Initialize the scene object to monitor changes in the external environment
|
||||
# 初始化场景对象，用来监听外部环境的变化
|
||||
self.scene = moveit_commander.PlanningSceneInterface()
|
||||
rospy.sleep(1)
|
||||
|
||||
# Initialize self.arm group in the robotic arm that needs to be controlled by move group
|
||||
# 初始化需要使用move group控制的机械臂中的self.arm group
|
||||
self.arm = moveit_commander.MoveGroupCommander("arm_group")
|
||||
|
||||
# 获取终端link的名称
|
||||
# Get the name of the terminal link,获取终端link的名称
|
||||
self.end_effector_link = self.arm.get_end_effector_link()
|
||||
|
||||
# Set the reference coordinate system used for the target position
|
||||
# 设置目标位置所使用的参考坐标系
|
||||
self.reference_frame = "joint1"
|
||||
self.arm.set_pose_reference_frame(self.reference_frame)
|
||||
|
||||
# 当运动规划失败后，允许重新规划
|
||||
# Allow replanning when motion planning fails,当运动规划失败后，允许重新规划
|
||||
self.arm.allow_replanning(True)
|
||||
|
||||
# Set the allowable error of position (unit: meter) and attitude (unit: radian)
|
||||
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
|
||||
self.arm.set_goal_position_tolerance(0.01)
|
||||
self.arm.set_goal_orientation_tolerance(0.05)
|
||||
|
||||
def moving(self):
|
||||
# # 控制机械臂先回到初始化位置
|
||||
# # Control the robotic arm to return to the initialization position first
|
||||
# 控制机械臂先回到初始化位置
|
||||
self.arm.set_named_target("init_pose")
|
||||
self.arm.go()
|
||||
rospy.sleep(2)
|
||||
|
||||
# Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates,
|
||||
# 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
|
||||
# Pose is described by quaternion, based on base_link coordinate system
|
||||
# 姿态使用四元数描述，基于base_link坐标系
|
||||
target_pose = PoseStamped()
|
||||
target_pose.header.frame_id = self.reference_frame
|
||||
|
|
@ -58,19 +65,23 @@ class MoveItPlanningDemo:
|
|||
target_pose.pose.orientation.z = 0.0
|
||||
target_pose.pose.orientation.w = 0.014
|
||||
|
||||
# Set the current state of the robot arm as the initial state of motion
|
||||
# 设置机器臂当前的状态作为运动初始状态
|
||||
self.arm.set_start_state_to_current_state()
|
||||
|
||||
# Set the target pose of the terminal motion of the robotic arm
|
||||
# 设置机械臂终端运动的目标位姿
|
||||
self.arm.set_pose_target(target_pose, self.end_effector_link)
|
||||
|
||||
# 规划运动路径
|
||||
# Plan the movement path,规划运动路径
|
||||
traj = self.arm.plan()
|
||||
|
||||
# Control the motion of the robotic arm according to the planned motion path
|
||||
# 按照规划的运动路径控制机械臂运动
|
||||
self.arm.execute(traj)
|
||||
rospy.sleep(1)
|
||||
|
||||
# Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy
|
||||
# 控制机械臂终端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy
|
||||
self.arm.shift_pose_target(1, 0.12, self.end_effector_link)
|
||||
self.arm.go()
|
||||
|
|
@ -80,6 +91,7 @@ class MoveItPlanningDemo:
|
|||
self.arm.go()
|
||||
rospy.sleep(1)
|
||||
|
||||
# Control the terminal of the robotic arm to rotate 90 degrees in the opposite direction. 0,1,2,3,4,5 represent xyzrpy
|
||||
# 控制机械臂终端反向旋转90度 0,1,2,3,4,5 代表xyzrpy
|
||||
# self.arm.shift_pose_target(3, -1.57, end_effector_link)
|
||||
# self.arm.go()
|
||||
|
|
@ -88,10 +100,10 @@ class MoveItPlanningDemo:
|
|||
def run(self):
|
||||
self.scene.remove_world_object("suit")
|
||||
|
||||
# 没有障碍物运行一次
|
||||
# Run once without obstacles,没有障碍物运行一次
|
||||
self.moving()
|
||||
|
||||
# 添加环境
|
||||
# Add environment,添加环境
|
||||
quat = quaternion_from_euler(3.1415, 0, -1.57)
|
||||
|
||||
suit_post = PoseStamped()
|
||||
|
|
@ -112,10 +124,10 @@ class MoveItPlanningDemo:
|
|||
self.scene.add_mesh("suit", suit_post, suit_path)
|
||||
rospy.sleep(2)
|
||||
|
||||
# 有环境影响后在运行一次
|
||||
# Run it again if there is an environmental impact,有环境影响后再运行一次
|
||||
self.moving()
|
||||
|
||||
# 关闭并退出moveit
|
||||
# close and exit moveit，关闭并退出moveit
|
||||
moveit_commander.roscpp_shutdown()
|
||||
moveit_commander.os._exit(0)
|
||||
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@ def listener():
|
|||
rospy.Subscriber("joint_states", JointState, callback)
|
||||
|
||||
# spin() simply keeps python from exiting until this node is stopped
|
||||
# spin() 只是阻止 python 退出，直到该节点停止
|
||||
rospy.spin()
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,8 +1,9 @@
|
|||
<launch>
|
||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/320_urdf/mycobot_pro_320.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_320)/config/mycobot_320.rviz" />
|
||||
<arg name="gui" default="true" />
|
||||
|
||||
<!-- Add model control,添加模型控制 -->
|
||||
<include file="$(find mycobot_280)/launch/slider_control.launch">
|
||||
<arg name="model" value="$(arg model)" />
|
||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
||||
|
|
|
|||
|
|
@ -1,11 +1,14 @@
|
|||
<launch>
|
||||
|
||||
<!-- By default, we do not start a database (it can be large) -->
|
||||
<!-- 默认情况下，我们不启动数据库（它可能很大） -->
|
||||
<arg name="db" default="false" />
|
||||
<!-- Allow user to specify database location -->
|
||||
<!-- 允许用户指定数据库位置 -->
|
||||
<arg name="db_path" default="$(find mycobot_320_moveit)/default_warehouse_mongo_db" />
|
||||
|
||||
<!-- By default, we are not in debug mode -->
|
||||
<!-- 默认情况下，我们不处于调试模式 -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!--
|
||||
|
|
@ -19,6 +22,7 @@
|
|||
<arg name="use_gui" default="false" />
|
||||
|
||||
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
|
||||
<!-- 加载参数服务器上的URDF、SRDF等.yaml配置文件-->
|
||||
<include file="$(find mycobot_320_moveit)/launch/planning_context.launch">
|
||||
<arg name="load_robot_description" value="true"/>
|
||||
</include>
|
||||
|
|
@ -27,15 +31,18 @@
|
|||
|
||||
|
||||
<!-- We do not have a robot connected, so publish fake joint states -->
|
||||
<!-- 我们没有连接机器人，所以发布假关节状态 -->
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
|
||||
<param name="use_gui" value="$(arg use_gui)"/>
|
||||
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- Given the published joint states, publish tf for the robot links -->
|
||||
<!-- 给定已发布的关节状态，为机器人链接发布 tf -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
|
||||
|
||||
<!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
|
||||
<!-- 运行主MoveIt！ 没有轨迹执行的可执行文件（我们默认没有配置控制器）-->
|
||||
<include file="$(find mycobot_320_moveit)/launch/move_group.launch">
|
||||
<arg name="allow_trajectory_execution" value="true"/>
|
||||
<arg name="fake_execution" value="true"/>
|
||||
|
|
@ -44,12 +51,14 @@
|
|||
</include>
|
||||
|
||||
<!-- Run Rviz and load the default config to see the state of the move_group node -->
|
||||
<!-- 运行 Rviz 并加载默认配置以查看 move_group 节点的状态 -->
|
||||
<include file="$(find mycobot_320_moveit)/launch/moveit_rviz.launch">
|
||||
<arg name="config" value="true"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
</include>
|
||||
|
||||
<!-- If database loading was enabled, start mongodb as well -->
|
||||
<!-- 如果启用了数据库加载，也启动 mongodb -->
|
||||
<include file="$(find mycobot_320_moveit)/launch/default_warehouse_db.launch" if="$(arg db)">
|
||||
<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
|
||||
</include>
|
||||
|
|
|
|||
|
|
@ -1,11 +1,13 @@
|
|||
<launch>
|
||||
|
||||
<!-- By default, we do not start a database (it can be large) -->
|
||||
<!-- 默认情况下，我们不启动数据库（它可能很大） -->
|
||||
<arg name="db" default="false" />
|
||||
<!-- Allow user to specify database location -->
|
||||
<!-- 允许用户指定数据库位置 -->
|
||||
<arg name="db_path" default="$(find mycobot_320_moveit)/default_warehouse_mongo_db" />
|
||||
|
||||
<!-- By default, we are not in debug mode -->
|
||||
<!-- By default, we are not in debug mode --> <!-- 默认情况下，我们不处于调试模式 -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!--
|
||||
|
|
@ -19,6 +21,7 @@
|
|||
<arg name="use_gui" default="false" />
|
||||
|
||||
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
|
||||
<!-- 加载参数服务器上的URDF、SRDF等.yaml配置文件-->
|
||||
<include file="$(find mycobot_320_moveit)/launch/planning_context.launch">
|
||||
<arg name="load_robot_description" value="true"/>
|
||||
</include>
|
||||
|
|
@ -27,15 +30,18 @@
|
|||
|
||||
|
||||
<!-- We do not have a robot connected, so publish fake joint states -->
|
||||
<!-- 我们没有连接机器人，所以发布假关节状态 -->
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
|
||||
<param name="use_gui" value="$(arg use_gui)"/>
|
||||
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- Given the published joint states, publish tf for the robot links -->
|
||||
<!-- 给定已发布的关节状态，为机器人链接发布 tf -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
|
||||
|
||||
<!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
|
||||
<!-- 运行主MoveIt！ 没有轨迹执行的可执行文件（我们默认没有配置控制器）-->
|
||||
<include file="$(find mycobot_320_moveit)/launch/move_group.launch">
|
||||
<arg name="allow_trajectory_execution" value="true"/>
|
||||
<arg name="fake_execution" value="true"/>
|
||||
|
|
@ -44,12 +50,14 @@
|
|||
</include>
|
||||
|
||||
<!-- Run Rviz and load the default config to see the state of the move_group node -->
|
||||
<!-- 运行 Rviz 并加载默认配置以查看 move_group 节点的状态 -->
|
||||
<include file="$(find mycobot_320_moveit)/launch/moveit_rviz.launch">
|
||||
<arg name="config" value="true"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
</include>
|
||||
|
||||
<!-- If database loading was enabled, start mongodb as well -->
|
||||
<!-- 如果启用了数据库加载，也启动 mongodb -->
|
||||
<include file="$(find mycobot_320_moveit)/launch/default_warehouse_db.launch" if="$(arg db)">
|
||||
<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
|
||||
</include>
|
||||
|
|
|
|||
|
|
@ -1,8 +1,10 @@
|
|||
<launch>
|
||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/600_urdf/mycobot_600_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_600)/config/mycobot_600.rviz" />
|
||||
<arg name="gui" default="true" />
|
||||
|
||||
|
||||
<!-- Add model control,添加模型控制 -->
|
||||
<include file="$(find mycobot_280)/launch/slider_control.launch">
|
||||
<arg name="model" value="$(arg model)" />
|
||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ mutex = Lock()
|
|||
|
||||
class ElephantRobot(object):
|
||||
def __init__(self, host, port):
|
||||
# setup connection
|
||||
# setup connection，建立连接
|
||||
self.BUFFSIZE = 2048
|
||||
self.ADDR = (host, port)
|
||||
self.tcp_client = socket(AF_INET, SOCK_STREAM)
|
||||
|
|
@ -119,6 +119,7 @@ class ElephantRobot(object):
|
|||
return self.string_to_int(res)
|
||||
|
||||
def program_run(self, start_line):
|
||||
"""run program，运行程序"""
|
||||
command = "program_run(" + str(start_line) + ")\n"
|
||||
res = self.send_command(command)
|
||||
return self.string_to_int(res)
|
||||
|
|
@ -129,6 +130,7 @@ class ElephantRobot(object):
|
|||
return res
|
||||
|
||||
def write_coords(self, coords, speed):
|
||||
"""set coords,设置坐标"""
|
||||
command = "set_coords("
|
||||
for item in coords:
|
||||
command += str(item) + ","
|
||||
|
|
@ -142,6 +144,7 @@ class ElephantRobot(object):
|
|||
self.write_coords(coords, speed)
|
||||
|
||||
def write_angles(self, angles, speed):
|
||||
"""set angles,设置角度"""
|
||||
command = "set_angles("
|
||||
for item in angles:
|
||||
command += str(item) + ","
|
||||
|
|
@ -243,6 +246,7 @@ old_list = []
|
|||
|
||||
|
||||
def callback(data):
|
||||
"""callback function,回调函数"""
|
||||
global old_list
|
||||
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
|
||||
print ("position", data.position)
|
||||
|
|
@ -271,7 +275,7 @@ def listener():
|
|||
ip = rospy.get_param("~ip", "192.168.10.169")
|
||||
print (ip)
|
||||
mc = ElephantRobot(ip, 5001)
|
||||
# START CLIENT
|
||||
# START CLIENT,启动客户端
|
||||
res = mc.start_client()
|
||||
if res != "":
|
||||
print res
|
||||
|
|
@ -285,6 +289,7 @@ def listener():
|
|||
rospy.Subscriber("joint_states", JointState, callback)
|
||||
|
||||
# spin() simply keeps python from exiting until this node is stopped
|
||||
# spin()只是阻止python退出，直到该节点停止
|
||||
print ("sping ...")
|
||||
rospy.spin()
|
||||
|
||||
|
|
|
|||
|
|
@ -1,8 +1,9 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<!-- Open communication service -->
|
||||
<!-- Open communication service ，开启通讯服务-->
|
||||
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_jsnn.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
|
|
|
|||
|
|
@ -1,8 +1,9 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<!-- Open communication service -->
|
||||
<!-- Open communication service ，开启通讯服务-->
|
||||
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_seeed.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
|
|
|
|||
|
|
@ -1,8 +1,9 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<!-- Open communication service -->
|
||||
<!-- Open communication service ，开启通讯服务-->
|
||||
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_services.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
|
|
|
|||
|
|
@ -1,8 +1,9 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<!-- Open communication service -->
|
||||
<!-- Open communication service，开启通讯服务 -->
|
||||
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
|
|
|
|||
|
|
@ -1,8 +1,9 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<!-- Open communication service -->
|
||||
<!-- Open communication service --><!-- 开启通讯服务 -->
|
||||
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_pi.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@ def create_services():
|
|||
|
||||
|
||||
def set_angles(req):
|
||||
"""set angles，设置角度"""
|
||||
angles = [
|
||||
req.joint_1,
|
||||
req.joint_2,
|
||||
|
|
@ -47,6 +48,7 @@ def set_angles(req):
|
|||
|
||||
|
||||
def get_angles(req):
|
||||
"""get angles,获取角度"""
|
||||
if mc:
|
||||
angles = mc.get_angles()
|
||||
return GetAnglesResponse(*angles)
|
||||
|
|
@ -77,6 +79,8 @@ def get_coords(req):
|
|||
|
||||
|
||||
def switch_status(req):
|
||||
"""Gripper switch status"""
|
||||
"""夹爪开关状态"""
|
||||
if mc:
|
||||
if req.Status:
|
||||
mc.set_gripper_state(0, 80)
|
||||
|
|
|
|||
|
|
@ -104,6 +104,8 @@ class MycobotTopics(object):
|
|||
sp.join()
|
||||
|
||||
def pub_real_angles(self):
|
||||
"""Publish real angle"""
|
||||
"""发布真实角度"""
|
||||
pub = rospy.Publisher("mycobot/angles_real",
|
||||
MycobotAngles, queue_size=5)
|
||||
ma = MycobotAngles()
|
||||
|
|
@ -122,6 +124,8 @@ class MycobotTopics(object):
|
|||
time.sleep(0.25)
|
||||
|
||||
def pub_real_coords(self):
|
||||
"""publish real coordinates"""
|
||||
"""发布真实坐标"""
|
||||
pub = rospy.Publisher("mycobot/coords_real",
|
||||
MycobotCoords, queue_size=5)
|
||||
ma = MycobotCoords()
|
||||
|
|
@ -141,6 +145,8 @@ class MycobotTopics(object):
|
|||
time.sleep(0.25)
|
||||
|
||||
def sub_set_angles(self):
|
||||
"""subscription angles"""
|
||||
"""订阅角度"""
|
||||
def callback(data):
|
||||
angles = [
|
||||
data.joint_1,
|
||||
|
|
@ -171,6 +177,8 @@ class MycobotTopics(object):
|
|||
rospy.spin()
|
||||
|
||||
def sub_gripper_status(self):
|
||||
"""Subscribe to Gripper Status"""
|
||||
"""订阅夹爪状态"""
|
||||
def callback(data):
|
||||
if data.Status:
|
||||
self.mc.set_gripper_state(0, 80)
|
||||
|
|
|
|||
|
|
@ -53,8 +53,9 @@ class Watcher:
|
|||
try:
|
||||
os.wait()
|
||||
except KeyboardInterrupt:
|
||||
# I put the capital B in KeyBoardInterrupt so I can
|
||||
# tell when the Watcher gets the SIGINT
|
||||
# I put the capital B in KeyBoardInterrupt so I can#
|
||||
# 我把大写的 B 放在 KeyBoardInterrupt 中，这样我就可以了
|
||||
# tell when the Watcher gets the SIGINT，告诉 Watcher 何时收到 SIGINT
|
||||
print("KeyBoardInterrupt")
|
||||
self.kill()
|
||||
sys.exit()
|
||||
|
|
@ -72,7 +73,7 @@ class MycobotTopics(object):
|
|||
|
||||
rospy.init_node("mycobot_topics")
|
||||
rospy.loginfo("start ...")
|
||||
# problem
|
||||
# Select connected device，选择连接设备
|
||||
port = rospy.get_param("~port", "/dev/ttyUSB0")
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
rospy.loginfo("%s,%s" % (port, baud))
|
||||
|
|
@ -112,6 +113,8 @@ class MycobotTopics(object):
|
|||
sp.join()
|
||||
|
||||
def pub_real_angles(self):
|
||||
"""Publish real angle"""
|
||||
"""发布真实角度"""
|
||||
pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5)
|
||||
ma = MycobotAngles()
|
||||
while not rospy.is_shutdown():
|
||||
|
|
@ -129,6 +132,8 @@ class MycobotTopics(object):
|
|||
time.sleep(0.25)
|
||||
|
||||
def pub_real_coords(self):
|
||||
"""publish real coordinates"""
|
||||
"""发布真实坐标"""
|
||||
pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5)
|
||||
ma = MycobotCoords()
|
||||
|
||||
|
|
@ -147,6 +152,8 @@ class MycobotTopics(object):
|
|||
time.sleep(0.25)
|
||||
|
||||
def sub_set_angles(self):
|
||||
"""subscription angles"""
|
||||
"""订阅角度"""
|
||||
def callback(data):
|
||||
angles = [
|
||||
data.joint_1,
|
||||
|
|
@ -165,6 +172,8 @@ class MycobotTopics(object):
|
|||
rospy.spin()
|
||||
|
||||
def sub_set_coords(self):
|
||||
"""Subscribe to coordinates"""
|
||||
"""订阅坐标"""
|
||||
def callback(data):
|
||||
angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
|
||||
sp = int(data.speed)
|
||||
|
|
@ -177,6 +186,8 @@ class MycobotTopics(object):
|
|||
rospy.spin()
|
||||
|
||||
def sub_gripper_status(self):
|
||||
"""Subscribe to Gripper Status"""
|
||||
"""订阅夹爪状态"""
|
||||
def callback(data):
|
||||
if data.Status:
|
||||
self.mc.set_gripper_state(0, 80)
|
||||
|
|
|
|||
|
|
@ -40,7 +40,8 @@ class Watcher:
|
|||
def __init__(self):
|
||||
"""Creates a child thread, which returns. The parent
|
||||
thread waits for a KeyboardInterrupt and then kills
|
||||
the child thread.
|
||||
the child thread.创建一个返回的子线程。 父线程等待 KeyboardInterrupt
|
||||
然后杀死子线程。
|
||||
"""
|
||||
self.child = os.fork()
|
||||
if self.child == 0:
|
||||
|
|
@ -108,6 +109,8 @@ class MycobotTopics(object):
|
|||
sp.join()
|
||||
|
||||
def pub_real_angles(self):
|
||||
"""Publish real angle"""
|
||||
"""发布真实角度"""
|
||||
pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5)
|
||||
ma = MycobotAngles()
|
||||
while not rospy.is_shutdown():
|
||||
|
|
@ -125,6 +128,8 @@ class MycobotTopics(object):
|
|||
time.sleep(0.25)
|
||||
|
||||
def pub_real_coords(self):
|
||||
"""publish real coordinates"""
|
||||
"""发布真实坐标"""
|
||||
pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5)
|
||||
ma = MycobotCoords()
|
||||
|
||||
|
|
@ -143,6 +148,8 @@ class MycobotTopics(object):
|
|||
time.sleep(0.25)
|
||||
|
||||
def sub_set_angles(self):
|
||||
"""subscription angles"""
|
||||
"""订阅角度"""
|
||||
def callback(data):
|
||||
angles = [
|
||||
data.joint_1,
|
||||
|
|
@ -173,6 +180,8 @@ class MycobotTopics(object):
|
|||
rospy.spin()
|
||||
|
||||
def sub_gripper_status(self):
|
||||
"""Subscribe to Gripper Status"""
|
||||
"""订阅夹爪状态"""
|
||||
def callback(data):
|
||||
if data.Status:
|
||||
self.mc.set_gripper_state(0, 80)
|
||||
|
|
|
|||
|
|
@ -106,6 +106,8 @@ class MycobotTopics(object):
|
|||
sp.join()
|
||||
|
||||
def pub_real_angles(self):
|
||||
"""Publish real angle"""
|
||||
"""发布真实角度"""
|
||||
pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5)
|
||||
ma = MycobotAngles()
|
||||
while not rospy.is_shutdown():
|
||||
|
|
@ -123,6 +125,8 @@ class MycobotTopics(object):
|
|||
time.sleep(0.25)
|
||||
|
||||
def pub_real_coords(self):
|
||||
"""publish real coordinates"""
|
||||
"""发布真实坐标"""
|
||||
pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5)
|
||||
ma = MycobotCoords()
|
||||
|
||||
|
|
@ -141,6 +145,8 @@ class MycobotTopics(object):
|
|||
time.sleep(0.25)
|
||||
|
||||
def sub_set_angles(self):
|
||||
"""subscription angles"""
|
||||
"""订阅角度"""
|
||||
def callback(data):
|
||||
angles = [
|
||||
data.joint_1,
|
||||
|
|
@ -171,6 +177,8 @@ class MycobotTopics(object):
|
|||
rospy.spin()
|
||||
|
||||
def sub_gripper_status(self):
|
||||
"""Subscribe to Gripper Status"""
|
||||
"""订阅夹爪状态"""
|
||||
def callback(data):
|
||||
if data.Status:
|
||||
self.mc.set_gripper_state(0, 80)
|
||||
|
|
|
|||
|
|
@ -79,6 +79,7 @@ def get_coords(req):
|
|||
|
||||
|
||||
def switch_status(req):
|
||||
"""Gripper switch,夹爪开关"""
|
||||
if mc:
|
||||
if req.Status:
|
||||
mc.set_gripper_state(0, 80)
|
||||
|
|
|
|||
|
|
@ -103,6 +103,8 @@ class MypalTopics(object):
|
|||
sp.join()
|
||||
|
||||
def pub_real_angles(self):
|
||||
"""Publish real angle"""
|
||||
"""发布真实角度"""
|
||||
pub = rospy.Publisher("Mypal/angles_real",
|
||||
MypalAngles, queue_size=5)
|
||||
ma = MypalAngles()
|
||||
|
|
@ -121,6 +123,8 @@ class MypalTopics(object):
|
|||
time.sleep(0.25)
|
||||
|
||||
def pub_real_coords(self):
|
||||
"""publish real coordinates"""
|
||||
"""发布真实坐标"""
|
||||
pub = rospy.Publisher("Mypal/coords_real",
|
||||
MypalCoords, queue_size=5)
|
||||
ma = MypalCoords()
|
||||
|
|
@ -140,6 +144,8 @@ class MypalTopics(object):
|
|||
time.sleep(0.25)
|
||||
|
||||
def sub_set_angles(self):
|
||||
"""subscription angles"""
|
||||
"""订阅角度"""
|
||||
def callback(data):
|
||||
angles = [
|
||||
data.joint_1,
|
||||
|
|
@ -172,6 +178,8 @@ class MypalTopics(object):
|
|||
rospy.spin()
|
||||
|
||||
def sub_gripper_status(self):
|
||||
"""Subscribe to Gripper Status"""
|
||||
"""订阅夹爪状态"""
|
||||
def callback(data):
|
||||
if data.Status:
|
||||
self.mc.set_gripper_state(0, 80)
|
||||
|
|
|
|||
|
|
@ -104,6 +104,8 @@ class MypalTopics(object):
|
|||
sp.join()
|
||||
|
||||
def pub_real_angles(self):
|
||||
"""Publish real angle"""
|
||||
"""发布真实角度"""
|
||||
pub = rospy.Publisher("Mypal/angles_real", MypalAngles, queue_size=5)
|
||||
ma = MypalAngles()
|
||||
while not rospy.is_shutdown():
|
||||
|
|
@ -121,6 +123,8 @@ class MypalTopics(object):
|
|||
time.sleep(0.25)
|
||||
|
||||
def pub_real_coords(self):
|
||||
"""publish real coordinates"""
|
||||
"""发布真实坐标"""
|
||||
pub = rospy.Publisher("Mypal/coords_real", MypalCoords, queue_size=5)
|
||||
ma = MypalCoords()
|
||||
|
||||
|
|
@ -139,6 +143,8 @@ class MypalTopics(object):
|
|||
time.sleep(0.25)
|
||||
|
||||
def sub_set_angles(self):
|
||||
"""subscription angles"""
|
||||
"""订阅角度"""
|
||||
def callback(data):
|
||||
angles = [
|
||||
data.joint_1,
|
||||
|
|
@ -169,6 +175,8 @@ class MypalTopics(object):
|
|||
rospy.spin()
|
||||
|
||||
def sub_gripper_status(self):
|
||||
"""Subscribe to Gripper Status"""
|
||||
"""订阅夹爪状态"""
|
||||
def callback(data):
|
||||
if data.Status:
|
||||
self.mc.set_gripper_state(0, 80)
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue