mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
And English annotation
This commit is contained in:
parent
ccdfb623ea
commit
46ceca5178
16 changed files with 107 additions and 78 deletions
|
|
@ -1,4 +1,5 @@
|
||||||
<launch>
|
<launch>
|
||||||
|
<!-- Load file model ,加载文件模型-->
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
<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="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
|
||||||
<arg name="gui" default="true" />
|
<arg name="gui" default="true" />
|
||||||
|
|
@ -10,6 +11,7 @@
|
||||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
||||||
<arg name="gui" value="$(arg gui)" />
|
<arg name="gui" value="$(arg gui)" />
|
||||||
</include>
|
</include>
|
||||||
|
<!-- vision node -->
|
||||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
<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="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
|
||||||
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
||||||
|
|
|
||||||
|
|
@ -1,7 +1,9 @@
|
||||||
<launch>
|
<launch>
|
||||||
|
<!-- Select connecting device and serial port ,选择连接设备及串口-->
|
||||||
<arg name="port" default="/dev/ttyUSB0" />
|
<arg name="port" default="/dev/ttyUSB0" />
|
||||||
<arg name="baud" default="115200" />
|
<arg name="baud" default="115200" />
|
||||||
|
|
||||||
|
<!-- Load file model ,加载文件模型-->
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
<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="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
|
||||||
<arg name="gui" default="false" />
|
<arg name="gui" default="false" />
|
||||||
|
|
@ -10,17 +12,17 @@
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
<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="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||||
<!-- Show in Rviz -->
|
<!-- Show in Rviz -->
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<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">
|
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
||||||
<arg name="port" value="$(arg port)" />
|
<arg name="port" value="$(arg port)" />
|
||||||
<arg name="baud" value="$(arg baud)" />
|
<arg name="baud" value="$(arg baud)" />
|
||||||
</include>
|
</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" />
|
<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
|
||||||
<!-- vision node -->
|
<!-- vision node -->
|
||||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
||||||
|
|
|
||||||
|
|
@ -1,11 +1,14 @@
|
||||||
<launch>
|
<launch>
|
||||||
|
<!-- Load file model ,加载文件模型-->
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260.urdf"/>
|
<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260.urdf"/>
|
||||||
<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mypal_260.rviz" />
|
<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mypal_260.rviz" />
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
<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" >
|
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
|
||||||
<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
|
<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
|
||||||
</node>
|
</node>
|
||||||
|
<!-- Show in Rviz ,显示在Rviz-->
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
</launch>
|
</launch>
|
||||||
|
|
|
||||||
|
|
@ -1,22 +1,24 @@
|
||||||
<launch>
|
<launch>
|
||||||
|
<!-- Select connecting device and serial port ,选择连接设备及串口-->
|
||||||
<arg name="port" default="/dev/ttyUSB0" />
|
<arg name="port" default="/dev/ttyUSB0" />
|
||||||
<arg name="baud" default="115200" />
|
<arg name="baud" default="115200" />
|
||||||
|
<!-- Load file model ,加载文件模型-->
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260.urdf"/>
|
<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260.urdf"/>
|
||||||
<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mypal_260.rviz" />
|
<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mypal_260.rviz" />
|
||||||
<arg name="gui" default="false" />
|
<arg name="gui" default="false" />
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
<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="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||||
<!-- Show in Rviz -->
|
<!-- Show in Rviz -->
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
|
<!-- mycobot-topics, mycobot-话题-->
|
||||||
<include file="$(find mypalletizer_communication)/launch/communication_service.launch">
|
<include file="$(find mypalletizer_communication)/launch/communication_service.launch">
|
||||||
<arg name="port" value="$(arg port)" />
|
<arg name="port" value="$(arg port)" />
|
||||||
<arg name="baud" value="$(arg baud)" />
|
<arg name="baud" value="$(arg baud)" />
|
||||||
</include>
|
</include>
|
||||||
|
<!-- listen and pub the real angles ,监听并发布真实角度-->
|
||||||
<node name="real_listener" pkg="mypalletizer_260" type="listen_real.py" />
|
<node name="real_listener" pkg="mypalletizer_260" type="listen_real.py" />
|
||||||
<node name="simple_gui" pkg="mypalletizer_260" type="simple_gui.py" />
|
<node name="simple_gui" pkg="mypalletizer_260" type="simple_gui.py" />
|
||||||
</launch>
|
</launch>
|
||||||
|
|
|
||||||
|
|
@ -1,14 +1,15 @@
|
||||||
<launch>
|
<launch>
|
||||||
<!-- <arg name="port" default="/dev/ttyUSB0" />
|
<!-- <arg name="port" default="/dev/ttyUSB0" />
|
||||||
<arg name="baud" default="115200" /> -->
|
<arg name="baud" default="115200" /> -->
|
||||||
|
|
||||||
|
<!-- Load file model ,加载文件模型-->
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260.urdf"/>
|
<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260.urdf"/>
|
||||||
<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mypal_260.rviz" />
|
<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mypal_260.rviz" />
|
||||||
<arg name="gui" default="true" />
|
<arg name="gui" default="true" />
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
<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="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">
|
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
|
||||||
<!-- <param name="use_gui" value="$(arg gui)" /> -->
|
<!-- <param name="use_gui" value="$(arg gui)" /> -->
|
||||||
|
|
@ -19,6 +20,6 @@
|
||||||
<param name="port" type="string" value="$(arg port)" />
|
<param name="port" type="string" value="$(arg port)" />
|
||||||
<param name="baud" type="int" value="$(arg baud)" />
|
<param name="baud" type="int" value="$(arg baud)" />
|
||||||
</node> -->
|
</node> -->
|
||||||
<!-- Show in Rviz -->
|
<!-- Show in Rviz,显示在Rviz -->
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
</launch>
|
</launch>
|
||||||
|
|
|
||||||
|
|
@ -1,7 +1,9 @@
|
||||||
<launch>
|
<launch>
|
||||||
|
<!-- Select connecting device and serial port ,选择连接设备及串口-->
|
||||||
<arg name="port" default="/dev/ttyUSB0" />
|
<arg name="port" default="/dev/ttyUSB0" />
|
||||||
<arg name="baud" default="115200" />
|
<arg name="baud" default="115200" />
|
||||||
|
|
||||||
|
<!-- Load file model ,加载文件模型-->
|
||||||
<!-- <arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mycobot_urdf.urdf"/>
|
<!-- <arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mycobot_urdf.urdf"/>
|
||||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" /> -->
|
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" /> -->
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260.urdf"/>
|
<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260.urdf"/>
|
||||||
|
|
@ -10,14 +12,17 @@
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
<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="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||||
<!-- Show in Rviz -->
|
<!-- Show in Rviz -->
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
|
|
||||||
|
<!-- mycobot-topics,mycobot-话题 -->
|
||||||
<include file="$(find mypalletizer_communication)/launch/communication_service.launch">
|
<include file="$(find mypalletizer_communication)/launch/communication_service.launch">
|
||||||
<arg name="port" value="$(arg port)" />
|
<arg name="port" value="$(arg port)" />
|
||||||
<arg name="baud" value="$(arg baud)" />
|
<arg name="baud" value="$(arg baud)" />
|
||||||
</include>
|
</include>
|
||||||
|
|
||||||
|
<!-- listen and pub the real angles ,监听并发布真实话题-->
|
||||||
<node name="real_listener" pkg="mypalletizer_260" type="listen_real.py" />
|
<node name="real_listener" pkg="mypalletizer_260" type="listen_real.py" />
|
||||||
</launch>
|
</launch>
|
||||||
|
|
|
||||||
|
|
@ -1,16 +1,17 @@
|
||||||
<launch>
|
<launch>
|
||||||
|
<!-- Load file model,加载文件模型 -->
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260.urdf"/>
|
<arg name="model" default="$(find mycobot_description)/urdf/260_urdf/mypal_260.urdf"/>
|
||||||
<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mypal_260.rviz" />
|
<arg name="rvizconfig" default="$(find mypalletizer_260)/config/mypal_260.rviz" />
|
||||||
<arg name="gui" default="true" />
|
<arg name="gui" default="true" />
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
<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="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
|
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
|
||||||
<param name="use_gui" value="$(arg gui)" />
|
<param name="use_gui" value="$(arg gui)" />
|
||||||
<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
|
<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
|
||||||
</node>
|
</node>
|
||||||
<!-- Show in Rviz -->
|
<!-- Show in Rviz,显示在Rviz -->
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
</launch>
|
</launch>
|
||||||
|
|
|
||||||
|
|
@ -27,7 +27,7 @@ class ImageConverter:
|
||||||
)
|
)
|
||||||
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
|
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
|
||||||
self.camera_matrix = None
|
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)
|
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
|
||||||
|
|
||||||
def callback(self, data):
|
def callback(self, data):
|
||||||
|
|
@ -37,7 +37,7 @@ class ImageConverter:
|
||||||
pose to transforming.
|
pose to transforming.
|
||||||
"""
|
"""
|
||||||
try:
|
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")
|
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
|
||||||
except CvBridgeError as e:
|
except CvBridgeError as e:
|
||||||
print(e)
|
print(e)
|
||||||
|
|
@ -45,7 +45,7 @@ class ImageConverter:
|
||||||
focal_length = size[1]
|
focal_length = size[1]
|
||||||
center = [size[1] / 2, size[0] / 2]
|
center = [size[1] / 2, size[0] / 2]
|
||||||
if self.camera_matrix is None:
|
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(
|
self.camera_matrix = np.array(
|
||||||
[
|
[
|
||||||
[focal_length, 0, center[0]],
|
[focal_length, 0, center[0]],
|
||||||
|
|
@ -55,7 +55,7 @@ class ImageConverter:
|
||||||
dtype=np.float32,
|
dtype=np.float32,
|
||||||
)
|
)
|
||||||
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
|
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)
|
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
|
||||||
corners, ids = ret[0], ret[1]
|
corners, ids = ret[0], ret[1]
|
||||||
# process marker data.
|
# process marker data.
|
||||||
|
|
@ -63,10 +63,10 @@ class ImageConverter:
|
||||||
if ids is not None:
|
if ids is not None:
|
||||||
# print('corners:', corners, 'ids:', ids)
|
# print('corners:', corners, 'ids:', ids)
|
||||||
|
|
||||||
# detect marker pose.
|
# detect marker pose. 检测标记姿势
|
||||||
# argument:
|
# argument:
|
||||||
# marker corners
|
# marker corners, 标记角
|
||||||
# marker size (meter)
|
# marker size (meter), 标记尺寸(米)
|
||||||
ret = cv.aruco.estimatePoseSingleMarkers(
|
ret = cv.aruco.estimatePoseSingleMarkers(
|
||||||
corners, 0.05, self.camera_matrix, self.dist_coeffs
|
corners, 0.05, self.camera_matrix, self.dist_coeffs
|
||||||
)
|
)
|
||||||
|
|
@ -75,7 +75,7 @@ class ImageConverter:
|
||||||
|
|
||||||
print("rvec:", rvec, "tvec:", tvec)
|
print("rvec:", rvec, "tvec:", tvec)
|
||||||
|
|
||||||
# just select first one detected marker.
|
# just select first one detected marker. 只需选择第一个检测到的标记
|
||||||
for i in range(rvec.shape[0]):
|
for i in range(rvec.shape[0]):
|
||||||
cv.aruco.drawDetectedMarkers(cv_image, corners)
|
cv.aruco.drawDetectedMarkers(cv_image, corners)
|
||||||
cv.aruco.drawAxis(
|
cv.aruco.drawAxis(
|
||||||
|
|
@ -90,14 +90,15 @@ class ImageConverter:
|
||||||
xyz = tvec[0, 0, :]
|
xyz = tvec[0, 0, :]
|
||||||
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
|
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
|
||||||
|
|
||||||
# get quaternion for ros.
|
# get quaternion for ros. 为ros获取四元数
|
||||||
euler = rvec[0, 0, :]
|
euler = rvec[0, 0, :]
|
||||||
tf_change = tf.transformations.quaternion_from_euler(
|
tf_change = tf.transformations.quaternion_from_euler(
|
||||||
euler[0], euler[1], euler[2]
|
euler[0], euler[1], euler[2]
|
||||||
)
|
)
|
||||||
print("tf_change:", tf_change)
|
print("tf_change:", tf_change)
|
||||||
|
|
||||||
# trans pose according [joint1]
|
# trans pose according [joint1]
|
||||||
|
# 根据 [joint1] 变换姿势
|
||||||
self.br.sendTransform(
|
self.br.sendTransform(
|
||||||
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
|
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
|
||||||
)
|
)
|
||||||
|
|
|
||||||
|
|
@ -5,18 +5,21 @@ from visualization_msgs.msg import Marker
|
||||||
import time
|
import time
|
||||||
import os
|
import os
|
||||||
|
|
||||||
|
# Type of message to communicate with mycobot
|
||||||
# 与 mycobot 通信的消息类型
|
# 与 mycobot 通信的消息类型
|
||||||
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
|
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
|
||||||
|
|
||||||
|
|
||||||
rospy.init_node("gipper_subscriber", anonymous=True)
|
rospy.init_node("gipper_subscriber", anonymous=True)
|
||||||
|
|
||||||
# 控制 mycobot 的 topic,依次是角度、坐标、夹爪
|
# Control the topic of mycobot, followed by angle, coordinates, gripper
|
||||||
|
# 控制mycobot的话题,依次是角度、坐标、夹爪
|
||||||
angle_pub = rospy.Publisher("mycobot/angles_goal",
|
angle_pub = rospy.Publisher("mycobot/angles_goal",
|
||||||
MycobotSetAngles, queue_size=5)
|
MycobotSetAngles, queue_size=5)
|
||||||
coord_pub = rospy.Publisher("mycobot/coords_goal",
|
coord_pub = rospy.Publisher("mycobot/coords_goal",
|
||||||
MycobotSetCoords, queue_size=5)
|
MycobotSetCoords, queue_size=5)
|
||||||
# Judging equipment: ttyUSB* is M5;ttyACM* is wio
|
# Judging device: ttyUSB* is M5;ttyACM* is wio
|
||||||
|
# 判断设备:ttyUSB*为M5;ttyACM*为wio
|
||||||
robot = os.popen("ls /dev/ttyUSB*").readline()
|
robot = os.popen("ls /dev/ttyUSB*").readline()
|
||||||
|
|
||||||
if "dev" in robot:
|
if "dev" in robot:
|
||||||
|
|
@ -27,27 +30,30 @@ else:
|
||||||
pump_pub = rospy.Publisher("mycobot/pump_status",
|
pump_pub = rospy.Publisher("mycobot/pump_status",
|
||||||
MycobotPumpStatus, queue_size=5)
|
MycobotPumpStatus, queue_size=5)
|
||||||
|
|
||||||
# 实例化消息对象
|
# Instantiate the message object. 实例化消息对象
|
||||||
angles = MycobotSetAngles()
|
angles = MycobotSetAngles()
|
||||||
coords = MycobotSetCoords()
|
coords = MycobotSetCoords()
|
||||||
pump = MycobotPumpStatus()
|
pump = MycobotPumpStatus()
|
||||||
|
|
||||||
# 与 mycobot 真实位置的偏差值
|
# Deviation value from the real position of mycobot
|
||||||
|
# 与mycobot真实位置的偏差值
|
||||||
x_offset = -20
|
x_offset = -20
|
||||||
y_offset = 20
|
y_offset = 20
|
||||||
z_offset = 110
|
z_offset = 110
|
||||||
|
|
||||||
# 通过该变量限制,抓取行为只做一次
|
# With this variable limit, the fetching behavior is only done once
|
||||||
|
# 使用此变量限制,抓取行为仅执行一次
|
||||||
flag = False
|
flag = False
|
||||||
|
|
||||||
# 为了后面比较二维码是否移动
|
# In order to compare whether the QR code moves later
|
||||||
|
# 为了比较二维码后面是否移动
|
||||||
temp_x = temp_y = temp_z = 0.0
|
temp_x = temp_y = temp_z = 0.0
|
||||||
|
|
||||||
temp_time = time.time()
|
temp_time = time.time()
|
||||||
|
|
||||||
|
|
||||||
def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
||||||
"""发布坐标"""
|
"""Post coordinates 发布坐标"""
|
||||||
coords.x = x
|
coords.x = x
|
||||||
coords.y = y
|
coords.y = y
|
||||||
coords.z = z
|
coords.z = z
|
||||||
|
|
@ -61,7 +67,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):
|
def pub_angles(a, b, c, d, e, f, sp):
|
||||||
"""发布角度"""
|
"""Publishing angle. 发布角度"""
|
||||||
angles.joint_1 = float(a)
|
angles.joint_1 = float(a)
|
||||||
angles.joint_2 = float(b)
|
angles.joint_2 = float(b)
|
||||||
angles.joint_3 = float(c)
|
angles.joint_3 = float(c)
|
||||||
|
|
@ -73,7 +79,7 @@ def pub_angles(a, b, c, d, e, f, sp):
|
||||||
|
|
||||||
|
|
||||||
def pub_pump(flag, Pin):
|
def pub_pump(flag, Pin):
|
||||||
"""发布夹爪状态"""
|
"""Publish gripper status. 发布夹爪状态"""
|
||||||
pump.Status = flag
|
pump.Status = flag
|
||||||
pump.Pin1 = Pin[0]
|
pump.Pin1 = Pin[0]
|
||||||
pump.Pin2 = Pin[1]
|
pump.Pin2 = Pin[1]
|
||||||
|
|
@ -81,7 +87,7 @@ def pub_pump(flag, Pin):
|
||||||
|
|
||||||
|
|
||||||
def target_is_moving(x, y, z):
|
def target_is_moving(x, y, z):
|
||||||
"""判断目标是否移动"""
|
"""Determine whether the target moves. 判断目标是否移动"""
|
||||||
count = 0
|
count = 0
|
||||||
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
|
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
|
||||||
print(o, n)
|
print(o, n)
|
||||||
|
|
@ -94,18 +100,19 @@ def target_is_moving(x, y, z):
|
||||||
|
|
||||||
|
|
||||||
def grippercallback(data):
|
def grippercallback(data):
|
||||||
"""回调函数"""
|
"""callback function. 回调函数"""
|
||||||
global flag, temp_x, temp_y, temp_z
|
global flag, temp_x, temp_y, temp_z
|
||||||
# rospy.loginfo('gripper_subscriber get date :%s', data)
|
# rospy.loginfo('gripper_subscriber get date :%s', data)
|
||||||
if flag:
|
if flag:
|
||||||
return
|
return
|
||||||
|
|
||||||
# 解析出坐标值
|
# Parse out the coordinate value. 解析出坐标值
|
||||||
# pump length: 88mm
|
# pump length: 88mm
|
||||||
x = float(format(data.pose.position.x * 1000, ".2f"))
|
x = float(format(data.pose.position.x * 1000, ".2f"))
|
||||||
y = float(format(data.pose.position.y * 1000, ".2f"))
|
y = float(format(data.pose.position.y * 1000, ".2f"))
|
||||||
z = float(format(data.pose.position.z * 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,或目标位置还在改变时,进行追踪行为
|
# 当运行时间小于 30s,或目标位置还在改变时,进行追踪行为
|
||||||
if (
|
if (
|
||||||
time.time() - temp_time < 30
|
time.time() - temp_time < 30
|
||||||
|
|
@ -120,7 +127,7 @@ def grippercallback(data):
|
||||||
|
|
||||||
temp_x, temp_y, temp_z = x, y, z
|
temp_x, temp_y, temp_z = x, y, z
|
||||||
return
|
return
|
||||||
else: # 表示目标处于静止状态,可以尝试抓取
|
else: # Indicates that the target is in a stationary state and can be attempted to grab. 表示目标处于静止状态,可以尝试抓取
|
||||||
|
|
||||||
print(x, y, z)
|
print(x, y, z)
|
||||||
|
|
||||||
|
|
@ -172,13 +179,13 @@ def main():
|
||||||
for _ in range(10):
|
for _ in range(10):
|
||||||
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
|
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
|
||||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||||
# pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
|
# pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
|
|
||||||
pub_pump(False, Pin)
|
pub_pump(False, Pin)
|
||||||
# time.sleep(2.5)
|
# time.sleep(2.5)
|
||||||
|
|
||||||
# mark 信息的订阅者
|
# subscribers to mark information, mark信息的订阅者
|
||||||
rospy.Subscriber("visualization_marker", Marker,
|
rospy.Subscriber("visualization_marker", Marker,
|
||||||
grippercallback, queue_size=1)
|
grippercallback, queue_size=1)
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -37,7 +37,7 @@ def talker():
|
||||||
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
|
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
|
||||||
rate = rospy.Rate(30) # 30hz
|
rate = rospy.Rate(30) # 30hz
|
||||||
|
|
||||||
# pub joint state
|
# pub joint state. 发布关节状态
|
||||||
joint_state_send = JointState()
|
joint_state_send = JointState()
|
||||||
joint_state_send.header = Header()
|
joint_state_send.header = Header()
|
||||||
|
|
||||||
|
|
@ -83,7 +83,7 @@ def talker():
|
||||||
marker_.scale.y = 0.04
|
marker_.scale.y = 0.04
|
||||||
marker_.scale.z = 0.04
|
marker_.scale.z = 0.04
|
||||||
|
|
||||||
# marker position initial
|
# marker position initial. 标记初始位置
|
||||||
# print(coords)
|
# print(coords)
|
||||||
if not coords:
|
if not coords:
|
||||||
# coords = [0, 0, 0, 0, 0 ]
|
# coords = [0, 0, 0, 0, 0 ]
|
||||||
|
|
|
||||||
|
|
@ -33,7 +33,7 @@ def talker():
|
||||||
print(type(trans), trans)
|
print(type(trans), trans)
|
||||||
print(type(rot), rot)
|
print(type(rot), rot)
|
||||||
|
|
||||||
# marker
|
# marker 标记
|
||||||
marker_.header.stamp = now
|
marker_.header.stamp = now
|
||||||
marker_.type = marker_.CUBE
|
marker_.type = marker_.CUBE
|
||||||
marker_.action = marker_.ADD
|
marker_.action = marker_.ADD
|
||||||
|
|
@ -41,7 +41,7 @@ def talker():
|
||||||
marker_.scale.y = 0.04
|
marker_.scale.y = 0.04
|
||||||
marker_.scale.z = 0.04
|
marker_.scale.z = 0.04
|
||||||
|
|
||||||
# marker position initial
|
# marker position initial. 标记初始位置
|
||||||
marker_.pose.position.x = trans[0]
|
marker_.pose.position.x = trans[0]
|
||||||
marker_.pose.position.y = trans[1]
|
marker_.pose.position.y = trans[1]
|
||||||
marker_.pose.position.z = trans[2]
|
marker_.pose.position.z = trans[2]
|
||||||
|
|
|
||||||
|
|
@ -17,7 +17,7 @@ def talker():
|
||||||
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||||
rate = rospy.Rate(30) # 30hz
|
rate = rospy.Rate(30) # 30hz
|
||||||
|
|
||||||
# pub joint state
|
# pub joint state 发布关节状态
|
||||||
joint_state_send = JointState()
|
joint_state_send = JointState()
|
||||||
joint_state_send.header = Header()
|
joint_state_send.header = Header()
|
||||||
|
|
||||||
|
|
@ -38,7 +38,7 @@ def talker():
|
||||||
|
|
||||||
rospy.loginfo("start loop ...")
|
rospy.loginfo("start loop ...")
|
||||||
while not rospy.is_shutdown():
|
while not rospy.is_shutdown():
|
||||||
# get real angles from server.
|
# get real angles from server. 从服务获取真实的角度
|
||||||
res = func()
|
res = func()
|
||||||
if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
|
if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
|
||||||
continue
|
continue
|
||||||
|
|
@ -52,7 +52,7 @@ def talker():
|
||||||
]
|
]
|
||||||
rospy.loginfo("res: {}".format(radians_list))
|
rospy.loginfo("res: {}".format(radians_list))
|
||||||
|
|
||||||
# publish angles.
|
# publish angles. 发布角度
|
||||||
joint_state_send.header.stamp = rospy.Time.now()
|
joint_state_send.header.stamp = rospy.Time.now()
|
||||||
joint_state_send.position = radians_list
|
joint_state_send.position = radians_list
|
||||||
pub.publish(joint_state_send)
|
pub.publish(joint_state_send)
|
||||||
|
|
|
||||||
|
|
@ -6,7 +6,7 @@ from sensor_msgs.msg import JointState
|
||||||
from std_msgs.msg import Header
|
from std_msgs.msg import Header
|
||||||
# from mycobot_communication.msg import MycobotAngles
|
# from mycobot_communication.msg import MycobotAngles
|
||||||
from mypalletizer_communication.msg import MypalAngles
|
from mypalletizer_communication.msg import MypalAngles
|
||||||
|
|
||||||
|
|
||||||
class Listener(object):
|
class Listener(object):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
|
|
@ -14,9 +14,9 @@ class Listener(object):
|
||||||
|
|
||||||
rospy.loginfo("start ...")
|
rospy.loginfo("start ...")
|
||||||
rospy.init_node("real_listener_1", anonymous=True)
|
rospy.init_node("real_listener_1", anonymous=True)
|
||||||
# init publisher.
|
# init publisher. 初始化发布者
|
||||||
self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||||
# init subscriber.
|
# init subscriber. 初始化订阅者
|
||||||
self.sub = rospy.Subscriber("mypal/angles_real", MypalAngles, self.callback)
|
self.sub = rospy.Subscriber("mypal/angles_real", MypalAngles, self.callback)
|
||||||
rospy.spin()
|
rospy.spin()
|
||||||
|
|
||||||
|
|
@ -26,7 +26,7 @@ class Listener(object):
|
||||||
Args:
|
Args:
|
||||||
data (MycobotAngles): callback argument.
|
data (MycobotAngles): callback argument.
|
||||||
"""
|
"""
|
||||||
# ini publisher object.
|
# ini publisher object. 初始化发布者对象
|
||||||
joint_state_send = JointState()
|
joint_state_send = JointState()
|
||||||
joint_state_send.header = Header()
|
joint_state_send.header = Header()
|
||||||
|
|
||||||
|
|
@ -43,7 +43,7 @@ class Listener(object):
|
||||||
joint_state_send.effort = []
|
joint_state_send.effort = []
|
||||||
joint_state_send.header.stamp = rospy.Time.now()
|
joint_state_send.header.stamp = rospy.Time.now()
|
||||||
|
|
||||||
# process callback data.
|
# process callback data. 处理回调数据
|
||||||
radians_list = [
|
radians_list = [
|
||||||
data.joint_1 * (math.pi / 180),
|
data.joint_1 * (math.pi / 180),
|
||||||
data.joint_2 * (math.pi / 180),
|
data.joint_2 * (math.pi / 180),
|
||||||
|
|
|
||||||
|
|
@ -10,47 +10,48 @@ from rospy import ServiceException
|
||||||
class Window:
|
class Window:
|
||||||
def __init__(self, handle):
|
def __init__(self, handle):
|
||||||
self.win = handle
|
self.win = handle
|
||||||
self.win.resizable(0, 0) # 固定窗口大小 fixed window size
|
self.win.resizable(0, 0) # fixed window size 固定窗口大小
|
||||||
|
|
||||||
self.model = 0
|
self.model = 0
|
||||||
self.speed = rospy.get_param("~speed", 50)
|
self.speed = rospy.get_param("~speed", 50)
|
||||||
|
|
||||||
# 设置默认速度 set default speed
|
# set default speed. 设置默认速度
|
||||||
self.speed_d = tk.StringVar()
|
self.speed_d = tk.StringVar()
|
||||||
self.speed_d.set(str(self.speed))
|
self.speed_d.set(str(self.speed))
|
||||||
# print(self.speed)
|
# print(self.speed)
|
||||||
self.connect_ser()
|
self.connect_ser()
|
||||||
|
|
||||||
# # 获取机械臂数据 get data
|
# Get robotic arm data. 获取机械臂数据
|
||||||
self.record_coords = [0, 0, 0, 0,self.speed, self.model]
|
self.record_coords = [0, 0, 0, 0,self.speed, self.model]
|
||||||
self.res_angles = [0, 0, 0, 0, self.speed, self.model]
|
self.res_angles = [0, 0, 0, 0, self.speed, self.model]
|
||||||
self.get_date()
|
self.get_date()
|
||||||
|
|
||||||
# get screen width and height
|
# get screen width and height. 获取屏幕宽度和高度
|
||||||
self.ws = self.win.winfo_screenwidth() # width of the screen
|
self.ws = self.win.winfo_screenwidth()
|
||||||
self.hs = self.win.winfo_screenheight() # height of the screen
|
self.hs = self.win.winfo_screenheight()
|
||||||
# calculate x and y coordinates for the Tk root window
|
# calculate x and y coordinates for the Tk root window
|
||||||
|
# 计算 Tk 根窗口的 x 和 y 坐标
|
||||||
x = (self.ws / 2) - 190
|
x = (self.ws / 2) - 190
|
||||||
y = (self.hs / 2) - 250
|
y = (self.hs / 2) - 250
|
||||||
self.win.geometry("430x350+{}+{}".format(x, y))
|
self.win.geometry("430x350+{}+{}".format(x, y))
|
||||||
# 布局 layout
|
# layout. 布局
|
||||||
self.set_layout()
|
self.set_layout()
|
||||||
# 输入部分 input
|
# input. 输入部分
|
||||||
self.need_input()
|
self.need_input()
|
||||||
# 展示部分 show info
|
# show info. 展示部分
|
||||||
self.show_init()
|
self.show_init()
|
||||||
|
|
||||||
# joint 设置按钮 Settings button
|
# Set joint button. 设置关节按钮
|
||||||
tk.Button(self.frmLT, text="Set", width=5, command=self.get_joint_input).grid(
|
tk.Button(self.frmLT, text="Set", width=5, command=self.get_joint_input).grid(
|
||||||
row=6, column=1, sticky="w", padx=3, pady=2
|
row=6, column=1, sticky="w", padx=3, pady=2
|
||||||
)
|
)
|
||||||
|
|
||||||
# coordination 设置按钮 Settings button
|
# Set Coordinate button. 设置坐标按钮
|
||||||
tk.Button(self.frmRT, text="Set", width=5, command=self.get_coord_input).grid(
|
tk.Button(self.frmRT, text="Set", width=5, command=self.get_coord_input).grid(
|
||||||
row=6, column=1, sticky="w", padx=3, pady=2
|
row=6, column=1, sticky="w", padx=3, pady=2
|
||||||
)
|
)
|
||||||
|
|
||||||
# 夹爪开关按钮 Gripper Switch
|
# Gripper Switch. 夹爪开关按钮
|
||||||
tk.Button(self.frmLB, text="Gripper Open", command=self.gripper_open, width=10).grid(
|
tk.Button(self.frmLB, text="Gripper Open", command=self.gripper_open, width=10).grid(
|
||||||
row=1, column=0, sticky="w", padx=3, pady=50
|
row=1, column=0, sticky="w", padx=3, pady=50
|
||||||
)
|
)
|
||||||
|
|
@ -91,7 +92,7 @@ class Window:
|
||||||
self.frmRT.grid(row=0, column=1, padx=2, pady=3)
|
self.frmRT.grid(row=0, column=1, padx=2, pady=3)
|
||||||
|
|
||||||
def need_input(self):
|
def need_input(self):
|
||||||
# 输入提示 input hint
|
# input hint. 输入提示
|
||||||
tk.Label(self.frmLT, text="Joint 1 ").grid(row=0)
|
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)
|
||||||
tk.Label(self.frmLT, text="Joint 3 ").grid(row=2)
|
tk.Label(self.frmLT, text="Joint 3 ").grid(row=2)
|
||||||
|
|
@ -104,7 +105,7 @@ class Window:
|
||||||
tk.Label(self.frmRT, text=" rx ").grid(row=3)
|
tk.Label(self.frmRT, text=" rx ").grid(row=3)
|
||||||
|
|
||||||
|
|
||||||
# 设置输入框的默认值 Set the default value of the input box
|
# Set the default value of the input box. 设置输入框的默认值
|
||||||
self.j1_default = tk.StringVar()
|
self.j1_default = tk.StringVar()
|
||||||
self.j1_default.set(self.res_angles[0])
|
self.j1_default.set(self.res_angles[0])
|
||||||
self.j2_default = tk.StringVar()
|
self.j2_default = tk.StringVar()
|
||||||
|
|
@ -125,7 +126,7 @@ class Window:
|
||||||
self.rx_default.set(self.record_coords[3])
|
self.rx_default.set(self.record_coords[3])
|
||||||
|
|
||||||
|
|
||||||
# joint input box 输入框
|
# joint input box. 输入框
|
||||||
self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default)
|
self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default)
|
||||||
self.J_1.grid(row=0, column=1, pady=3)
|
self.J_1.grid(row=0, column=1, pady=3)
|
||||||
self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default)
|
self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default)
|
||||||
|
|
@ -136,7 +137,7 @@ class Window:
|
||||||
self.J_4.grid(row=3, column=1, pady=3)
|
self.J_4.grid(row=3, column=1, pady=3)
|
||||||
|
|
||||||
|
|
||||||
# coord input box 输入框
|
# coord input box. 输入框
|
||||||
self.x = tk.Entry(self.frmRT, textvariable=self.x_default)
|
self.x = tk.Entry(self.frmRT, textvariable=self.x_default)
|
||||||
self.x.grid(row=0, column=1, pady=3, padx=0)
|
self.x.grid(row=0, column=1, pady=3, padx=0)
|
||||||
self.y = tk.Entry(self.frmRT, textvariable=self.y_default)
|
self.y = tk.Entry(self.frmRT, textvariable=self.y_default)
|
||||||
|
|
@ -147,13 +148,13 @@ class Window:
|
||||||
self.rx.grid(row=3, column=1, pady=3)
|
self.rx.grid(row=3, column=1, pady=3)
|
||||||
|
|
||||||
|
|
||||||
# 所有输入框,用于拿输入的数据 all input box
|
# 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.all_j = [self.J_1, self.J_2, self.J_3, self.J_4]
|
||||||
self.all_c = [self.x, self.y, self.z, self.rx]
|
self.all_c = [self.x, self.y, self.z, self.rx]
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
# 速度输入框 speed input box
|
# speed input box. 速度输入框
|
||||||
tk.Label(
|
tk.Label(
|
||||||
self.frmLB,
|
self.frmLB,
|
||||||
text="speed",
|
text="speed",
|
||||||
|
|
@ -162,7 +163,7 @@ class Window:
|
||||||
self.get_speed.grid(row=0, column=1)
|
self.get_speed.grid(row=0, column=1)
|
||||||
|
|
||||||
def show_init(self):
|
def show_init(self):
|
||||||
# 显示 display
|
# display. 显示
|
||||||
tk.Label(self.frmLC, text="Joint 1 ").grid(row=0)
|
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)
|
||||||
tk.Label(self.frmLC, text="Joint 3 ").grid(row=2)
|
tk.Label(self.frmLC, text="Joint 3 ").grid(row=2)
|
||||||
|
|
@ -228,9 +229,9 @@ class Window:
|
||||||
|
|
||||||
]
|
]
|
||||||
|
|
||||||
# 显示
|
# display. 显示
|
||||||
tk.Label(self.frmLC, text=" x ").grid(row=0, column=3)
|
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) # the second row. 第二行
|
||||||
tk.Label(self.frmLC, text=" z ").grid(row=2, 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=" rx ").grid(row=3, column=3)
|
||||||
|
|
||||||
|
|
@ -286,7 +287,7 @@ class Window:
|
||||||
bg="white",
|
bg="white",
|
||||||
).grid(row=3, column=4, padx=5, pady=5)
|
).grid(row=3, column=4, padx=5, pady=5)
|
||||||
|
|
||||||
# mm 单位展示 show label"mm"
|
# show label"mm" .mm 单位展示
|
||||||
self.unit = tk.StringVar()
|
self.unit = tk.StringVar()
|
||||||
self.unit.set("mm")
|
self.unit.set("mm")
|
||||||
for i in range(4):
|
for i in range(4):
|
||||||
|
|
@ -307,6 +308,7 @@ class Window:
|
||||||
pass
|
pass
|
||||||
|
|
||||||
def get_coord_input(self):
|
def get_coord_input(self):
|
||||||
|
# Get the data input by coord and send it to the robotic arm
|
||||||
# 获取 coord 输入的数据,发送给机械臂
|
# 获取 coord 输入的数据,发送给机械臂
|
||||||
c_value = []
|
c_value = []
|
||||||
for i in self.all_c:
|
for i in self.all_c:
|
||||||
|
|
@ -325,7 +327,8 @@ class Window:
|
||||||
self.show_j_date(c_value[:-2], "coord")
|
self.show_j_date(c_value[:-2], "coord")
|
||||||
|
|
||||||
def get_joint_input(self):
|
def get_joint_input(self):
|
||||||
# 获取joint输入的数据,发送给机械臂 Get the data of the joint and send it to the robotic arm
|
# Get the data of the joint and send it to the robotic arm
|
||||||
|
# 获取joint输入的数据,发送给机械臂
|
||||||
j_value = []
|
j_value = []
|
||||||
for i in self.all_j:
|
for i in self.all_j:
|
||||||
# print(type(i.get()))
|
# print(type(i.get()))
|
||||||
|
|
@ -343,7 +346,7 @@ class Window:
|
||||||
# return j_value,c_value,speed
|
# return j_value,c_value,speed
|
||||||
|
|
||||||
def get_date(self):
|
def get_date(self):
|
||||||
# 拿机械臂的数据,用于展示 Get the data of robotic arm for display
|
# Get the data of robotic arm for display. 拿机械臂的数据,用于展示
|
||||||
t = time.time()
|
t = time.time()
|
||||||
while time.time() - t < 2:
|
while time.time() - t < 2:
|
||||||
self.res = self.get_coords()
|
self.res = self.get_coords()
|
||||||
|
|
@ -380,7 +383,7 @@ class Window:
|
||||||
|
|
||||||
# def send_input(self,dates):
|
# def send_input(self,dates):
|
||||||
def show_j_date(self, date, way=""):
|
def show_j_date(self, date, way=""):
|
||||||
# 展示数据
|
# Show data. 展示数据
|
||||||
if way == "coord":
|
if way == "coord":
|
||||||
for i, j in zip(date, self.coord_all):
|
for i, j in zip(date, self.coord_all):
|
||||||
# print(i)
|
# print(i)
|
||||||
|
|
|
||||||
|
|
@ -25,7 +25,7 @@ def callback(data):
|
||||||
data_list = []
|
data_list = []
|
||||||
for index, value in enumerate(data.position):
|
for index, value in enumerate(data.position):
|
||||||
data_list.append(value)
|
data_list.append(value)
|
||||||
del data_list[3] # delete the angle of joint3 to joint4,because it do not exsist actually!
|
del data_list[3] # delete the angle of joint3 to joint4,because it do not exsist actually! 把joint3到joint4的角度删掉,因为它实际上不存在!
|
||||||
# data_list[3] = data_list[4]
|
# data_list[3] = data_list[4]
|
||||||
# print(data_list)
|
# print(data_list)
|
||||||
mc.send_radians(data_list, 80)
|
mc.send_radians(data_list, 80)
|
||||||
|
|
@ -37,12 +37,13 @@ def listener():
|
||||||
rospy.init_node("control_slider", anonymous=True)
|
rospy.init_node("control_slider", anonymous=True)
|
||||||
|
|
||||||
rospy.Subscriber("joint_states", JointState, callback)
|
rospy.Subscriber("joint_states", JointState, callback)
|
||||||
port = rospy.get_param("~port", "/dev/ttyUSB0")
|
port = rospy.get_param("~port", "/dev/ttyUSB0") # Select connected device. 选择连接设备
|
||||||
baud = rospy.get_param("~baud", 115200)
|
baud = rospy.get_param("~baud", 115200)
|
||||||
print(port, baud)
|
print(port, baud)
|
||||||
mc = MyPalletizer(port, baud)
|
mc = MyPalletizer(port, baud)
|
||||||
|
|
||||||
# spin() simply keeps python from exiting until this node is stopped
|
# spin() simply keeps python from exiting until this node is stopped
|
||||||
|
# spin() 只是阻止python退出,直到该节点停止
|
||||||
print("spin ...")
|
print("spin ...")
|
||||||
rospy.spin()
|
rospy.spin()
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -11,7 +11,7 @@ import time
|
||||||
|
|
||||||
import roslib
|
import roslib
|
||||||
|
|
||||||
|
# Terminal output prompt information. 终端输出提示信息
|
||||||
msg = """\
|
msg = """\
|
||||||
Mypalletizer Teleop Keyboard Controller
|
Mypalletizer Teleop Keyboard Controller
|
||||||
---------------------------
|
---------------------------
|
||||||
|
|
@ -100,6 +100,7 @@ def teleop_keyboard():
|
||||||
try:
|
try:
|
||||||
print(msg)
|
print(msg)
|
||||||
print(vels(speed, change_percent))
|
print(vels(speed, change_percent))
|
||||||
|
# Keyboard keys call different motion functions. 键盘按键调用不同的运动功能
|
||||||
while 1:
|
while 1:
|
||||||
try:
|
try:
|
||||||
# print("\r current coords: %s" % record_coords, end="")
|
# print("\r current coords: %s" % record_coords, end="")
|
||||||
|
|
|
||||||
Loading…
Add table
Reference in a new issue