kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-05 19:47:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

删除弃用文件

Browse source
This commit is contained in:
wangWking 2024-09-12 18:44:26 +08:00
parent 5157ee03e1
commit aba3c6ee55
95 changed files with 0 additions and 7895 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
mecharm/mecharm/CMakeLists.txt
View file

@ -27,9 +27,6 @@ catkin_install_python(PROGRAMS
scripts/teleop_keyboard.py
scripts/listen_real.py
scripts/listen_real_of_topic.py
scripts/detect_marker.py
scripts/following_marker.py
scripts/follow_and_pump.py
scripts/simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

216
mecharm/mecharm/config/mycobot_with_marker.rviz
View file

@ -1,216 +0,0 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
- /TF1
- /Marker1
- /Marker1/Namespaces1
Splitter Ratio: 0.5
Tree Height: 775
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
joint1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint6_flange:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
basic_shapes:
Value: true
joint1:
Value: true
joint2:
Value: true
joint3:
Value: true
joint4:
Value: true
joint5:
Value: true
joint6:
Value: true
joint6_flange:
Value: true
Marker Scale: 0.300000012
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
joint1:
joint2:
joint3:
joint4:
joint5:
joint6:
joint6_flange:
basic_shapes:
{}
Update Interval: 0
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /visualization_marker
Name: Marker
Namespaces:
basic_cube: true
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: joint1
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.11990476
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.0706475973
Y: -0.0814988762
Z: 0.107583851
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.375398338
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.235389769
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1855
X: 65
Y: 24

18
mecharm/mecharm/launch/detect_marker.launch
View file

@ -1,18 +0,0 @@
<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" />
<arg name="num" default="0" />
<include file="$(find mycobot_280)/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_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" />
</launch>

31
mecharm/mecharm/launch/detect_marker_with_topic.launch
View file

@ -1,31 +0,0 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyAMA0" />
<arg name="baud" default="1000000" />
<!-- 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" />
<arg name="num" default="0" />
<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" />
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- 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，坚听并发布真实的角度 -->
<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)"/>
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
</launch>

125
mecharm/mecharm/scripts/detect_marker.py
View file

@ -1,125 +0,0 @@
#!/usr/bin/env python
# -*- coding:utf-8 -*-
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in. 订阅者，监听是否有img进来
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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. 如果没有就计算相机矩阵
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# 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.
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose. 检测标记姿势
# argument:
# marker corners, 标记角
# marker size (meter), 标记尺寸（米）
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker. 只需选择第一个检测到的标记
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# 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]
# 根据 [joint1] 变换姿势
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

197
mecharm/mecharm/scripts/follow_and_pump.py
View file

@ -1,197 +0,0 @@
#!/usr/bin/env python2
# coding:utf-8
import rospy
from visualization_msgs.msg import Marker
import time
import os
# Type of message to communicate 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的话题，依次是角度、坐标、夹爪
angle_pub = rospy.Publisher("mycobot/angles_goal",
MycobotSetAngles, queue_size=5)
coord_pub = rospy.Publisher("mycobot/coords_goal",
MycobotSetCoords, queue_size=5)
# Judging device: ttyUSB* is M5；ttyACM* is wio
# 判断设备：ttyUSB*为M5；ttyACM*为wio
robot = os.popen("ls /dev/ttyAMA*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# Instantiate the message object. 实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# Deviation value from the real position of mycobot
# 与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
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
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)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""Publish gripper status. 发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
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)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
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
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: # Indicates that the target is in a stationary state and can be attempted to grab. 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# subscribers to mark information, mark信息的订阅者
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

65
mecharm/mecharm/scripts/following_marker.py
View file

@ -1,65 +0,0 @@
#!/usr/bin/env python2
# -*- coding:utf-8 -*-
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/joint1"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker 标记
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial. 标记初始位置
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass

3
mecharm/mecharm_pi/CMakeLists.txt
View file

@ -27,9 +27,6 @@ catkin_install_python(PROGRAMS
scripts/teleop_keyboard.py
scripts/listen_real.py
scripts/listen_real_of_topic.py
scripts/detect_marker.py
scripts/following_marker.py
scripts/follow_and_pump.py
scripts/simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

18
mecharm/mecharm_pi/launch/detect_marker.launch
View file

@ -1,18 +0,0 @@
<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" />
<arg name="num" default="0" />
<include file="$(find mycobot_280)/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_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" />
</launch>

31
mecharm/mecharm_pi/launch/detect_marker_with_topic.launch
View file

@ -1,31 +0,0 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyAMA0" />
<arg name="baud" default="1000000" />
<!-- 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" />
<arg name="num" default="0" />
<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" />
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- 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，坚听并发布真实的角度 -->
<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)"/>
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
</launch>

125
mecharm/mecharm_pi/scripts/detect_marker.py
View file

@ -1,125 +0,0 @@
#!/usr/bin/env python
# -*- coding:utf-8 -*-
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in. 订阅者，监听是否有img进来
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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. 如果没有就计算相机矩阵
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# 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.
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose. 检测标记姿势
# argument:
# marker corners, 标记角
# marker size (meter), 标记尺寸（米）
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker. 只需选择第一个检测到的标记
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# 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]
# 根据 [joint1] 变换姿势
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

197
mecharm/mecharm_pi/scripts/follow_and_pump.py
View file

@ -1,197 +0,0 @@
#!/usr/bin/env python2
# coding:utf-8
import rospy
from visualization_msgs.msg import Marker
import time
import os
# Type of message to communicate 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的话题，依次是角度、坐标、夹爪
angle_pub = rospy.Publisher("mycobot/angles_goal",
MycobotSetAngles, queue_size=5)
coord_pub = rospy.Publisher("mycobot/coords_goal",
MycobotSetCoords, queue_size=5)
# Judging device: ttyUSB* is M5；ttyACM* is wio
# 判断设备：ttyUSB*为M5；ttyACM*为wio
robot = os.popen("ls /dev/ttyAMA*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# Instantiate the message object. 实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# Deviation value from the real position of mycobot
# 与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
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
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)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""Publish gripper status. 发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
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)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
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
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: # Indicates that the target is in a stationary state and can be attempted to grab. 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# subscribers to mark information, mark信息的订阅者
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

65
mecharm/mecharm_pi/scripts/following_marker.py
View file

@ -1,65 +0,0 @@
#!/usr/bin/env python2
# -*- coding:utf-8 -*-
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/joint1"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker 标记
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial. 标记初始位置
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass

3
myArm/myarm/CMakeLists.txt
View file

@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS
scripts/teleop_keyboard.py
scripts/listen_real.py
scripts/listen_real_of_topic.py
scripts/detect_marker.py
scripts/following_marker.py
scripts/follow_and_pump.py
scripts/simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

216
myArm/myarm/config/mycobot_with_marker.rviz
View file

@ -1,216 +0,0 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
- /TF1
- /Marker1
- /Marker1/Namespaces1
Splitter Ratio: 0.5
Tree Height: 775
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
joint1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint6_flange:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
basic_shapes:
Value: true
joint1:
Value: true
joint2:
Value: true
joint3:
Value: true
joint4:
Value: true
joint5:
Value: true
joint6:
Value: true
joint6_flange:
Value: true
Marker Scale: 0.300000012
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
joint1:
joint2:
joint3:
joint4:
joint5:
joint6:
joint6_flange:
basic_shapes:
{}
Update Interval: 0
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /visualization_marker
Name: Marker
Namespaces:
basic_cube: true
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: joint1
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.11990476
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.0706475973
Y: -0.0814988762
Z: 0.107583851
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.375398338
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.235389769
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1855
X: 65
Y: 24

18
myArm/myarm/launch/detect_marker.launch
View file

@ -1,18 +0,0 @@
<launch>
<!-- Load file model ，加载文件模型-->
<arg name="model" default="$(find mycobot_description)/urdf/myarm_300_pi/myarm_300_pi.urdf"/>
<arg name="rvizconfig" default="$(find myarm)/config/mycobot_with_marker.rviz" />
<arg name="gui" default="true" />
<arg name="num" default="0" />
<include file="$(find myarm)/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_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" />
</launch>

30
myArm/myarm/launch/detect_marker_with_topic.launch
View file

@ -1,30 +0,0 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyAMA0" />
<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" />
<arg name="num" default="0" />
<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" />
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- 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 ，监听并发布真实的角度-->
<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)"/>
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
</launch>

123
myArm/myarm/scripts/detect_marker.py
View file

@ -1,123 +0,0 @@
#!/usr/bin/env python
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in. 订阅者，监听是否有img
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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.如果没有，则计算相机矩阵
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# 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.处理标记数据
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose. 检测marker位姿。
# argument:
# marker corners,标记角
# marker size (meter),标记尺寸（米）
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker.只需选择第一个检测到的标记。
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# 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]，根据 [joint1] 变换姿势
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

194
myArm/myarm/scripts/follow_and_pump.py
View file

@ -1,194 +0,0 @@
#!/usr/bin/env python2
# coding:utf-8
import rospy
from visualization_msgs.msg import Marker
import time
import os
# 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)
# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
robot = os.popen("ls /dev/ttyUSB*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# instantiate the message object，实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# 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
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
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)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""Publish gripper status,发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
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)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
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
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# mark 信息的订阅者,subscribers to mark information
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

64
myArm/myarm/scripts/following_marker.py
View file

@ -1,64 +0,0 @@
#!/usr/bin/env python2
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/base"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("base", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial,标记位置初始化
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass

3
myArm/myarm_c650/CMakeLists.txt
View file

@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS
scripts/teleop_keyboard.py
scripts/listen_real.py
scripts/listen_real_of_topic.py
scripts/detect_marker.py
scripts/following_marker.py
scripts/follow_and_pump.py
scripts/simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

18
myArm/myarm_c650/launch/detect_marker.launch
View file

@ -1,18 +0,0 @@
<launch>
<!-- Load file model ，加载文件模型-->
<arg name="model" default="$(find mycobot_description)/urdf/myarm_c650/myarm_c650.urdf"/>
<arg name="rvizconfig" default="$(find myarm_c650)/config/myarm_with_marker.rviz" />
<arg name="gui" default="true" />
<arg name="num" default="0" />
<include file="$(find myarm_c650)/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="myarm_c650" type="opencv_camera" args="$(arg num)"/>
<node name="detect_marker" pkg="myarm_c650" type="detect_marker.py" />
<node name="following_marker" pkg="myarm_c650" type="following_marker.py" />
</launch>

30
myArm/myarm_c650/launch/detect_marker_with_topic.launch
View file

@ -1,30 +0,0 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyACM0" />
<arg name="baud" default="1000000" />
<!-- Load file model ，加载文件模型-->
<arg name="model" default="$(find mycobot_description)/urdf/myarm_c650/myarm_urdf.urdf"/>
<arg name="rvizconfig" default="$(find myarm_c650)/config/myarm_with_marker.rviz" />
<arg name="gui" default="false" />
<arg name="num" default="0" />
<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" />
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- mycobot-topics ，mycobot-话题-->
<include file="$(find myarm_communication)/launch/communication_topic.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="myarm_c650" type="listen_real_of_topic.py" />
<!-- vision node -->
<node name="opencv_camera" pkg="myarm_c650" type="opencv_camera" args="$(arg num)"/>
<node name="detect_marker" pkg="myarm_c650" type="detect_marker.py" />
<node name="following_marker" pkg="myarm_c650" type="following_marker.py" />
</launch>

123
myArm/myarm_c650/scripts/detect_marker.py
View file

@ -1,123 +0,0 @@
#!/usr/bin/env python
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in. 订阅者，监听是否有img
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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.如果没有，则计算相机矩阵
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# 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.处理标记数据
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose. 检测marker位姿。
# argument:
# marker corners,标记角
# marker size (meter),标记尺寸（米）
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker.只需选择第一个检测到的标记。
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# 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]，根据 [joint1] 变换姿势
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

194
myArm/myarm_c650/scripts/follow_and_pump.py
View file

@ -1,194 +0,0 @@
#!/usr/bin/env python2
# coding:utf-8
import rospy
from visualization_msgs.msg import Marker
import time
import os
# 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)
# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
robot = os.popen("ls /dev/ttyUSB*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# instantiate the message object，实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# 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
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
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)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""Publish gripper status,发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
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)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
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
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# mark 信息的订阅者,subscribers to mark information
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

64
myArm/myarm_c650/scripts/following_marker.py
View file

@ -1,64 +0,0 @@
#!/usr/bin/env python2
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/base"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("base", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial,标记位置初始化
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass

3
myArm/myarm_m/CMakeLists.txt
View file

@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS
scripts/teleop_keyboard.py
scripts/listen_real.py
scripts/listen_real_of_topic.py
scripts/detect_marker.py
scripts/following_marker.py
scripts/follow_and_pump.py
scripts/simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

18
myArm/myarm_m/launch/detect_marker.launch
View file

@ -1,18 +0,0 @@
<launch>
<!-- Load file model ，加载文件模型-->
<arg name="model" default="$(find mycobot_description)/urdf/myarm_300_pi/myarm_300_pi.urdf"/>
<arg name="rvizconfig" default="$(find myarm)/config/mycobot_with_marker.rviz" />
<arg name="gui" default="true" />
<arg name="num" default="0" />
<include file="$(find myarm)/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_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" />
</launch>

30
myArm/myarm_m/launch/detect_marker_with_topic.launch
View file

@ -1,30 +0,0 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyAMA0" />
<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" />
<arg name="num" default="0" />
<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" />
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- 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 ，监听并发布真实的角度-->
<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)"/>
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
</launch>

123
myArm/myarm_m/scripts/detect_marker.py
View file

@ -1,123 +0,0 @@
#!/usr/bin/env python
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in. 订阅者，监听是否有img
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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.如果没有，则计算相机矩阵
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# 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.处理标记数据
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose. 检测marker位姿。
# argument:
# marker corners,标记角
# marker size (meter),标记尺寸（米）
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker.只需选择第一个检测到的标记。
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# 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]，根据 [joint1] 变换姿势
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

194
myArm/myarm_m/scripts/follow_and_pump.py
View file

@ -1,194 +0,0 @@
#!/usr/bin/env python2
# coding:utf-8
import rospy
from visualization_msgs.msg import Marker
import time
import os
# 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)
# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
robot = os.popen("ls /dev/ttyUSB*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# instantiate the message object，实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# 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
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
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)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""Publish gripper status,发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
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)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
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
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# mark 信息的订阅者,subscribers to mark information
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

64
myArm/myarm_m/scripts/following_marker.py
View file

@ -1,64 +0,0 @@
#!/usr/bin/env python2
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/base"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("base", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial,标记位置初始化
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass

163
myArm/myarm_moveit/scripts/obstacle_avoidance_demo.py
View file

@ -1,163 +0,0 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import tf
class MoveItPlanningDemo:
def __init__(self):
rospy.init_node('moveit_avoid_obstacles', anonymous=True)
# 初始化MoveIt
moveit_commander.roscpp_initialize(sys.argv)
# 创建RobotCommander对象
self.robot = moveit_commander.RobotCommander()
# 创建PlanningSceneInterface对象
self.scene = moveit_commander.PlanningSceneInterface()
# 创建MoveGroupCommander对象
self.arm_group = moveit_commander.MoveGroupCommander("arm_group")
# 获取末端关节的名称
self.end_effector_link = self.arm_group.get_end_effector_link()
# 设置目标位置所使用的坐标参考系
self.reference_frame = 'base'
self.arm_group.set_pose_reference_frame(self.reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
self.arm_group.set_goal_position_tolerance(0.01)
self.arm_group.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
self.arm_group.allow_replanning(True)
# 设置规划的最大时间为20秒
self.arm_group.set_planning_time(20)
# 设置规划尝试次数为10次（或者更大的值）
self.arm_group.set_num_planning_attempts(20)
def add_scene(self):
# 添加第一个圆柱作为障碍物（垂直于平面）
cylinder1_pose = geometry_msgs.msg.PoseStamped()
cylinder1_pose.header.frame_id = self.robot.get_planning_frame()
cylinder1_pose.pose.position.x = 0.15
cylinder1_pose.pose.position.y = 0
cylinder1_pose.pose.position.z = 0.30
cylinder1_pose.pose.orientation.w = 1.0
self.scene.add_cylinder("cylinder1", cylinder1_pose, height=0.6, radius=0.01)
# 添加第二个圆柱作为障碍物（水平于平面，构成十字架）
cylinder2_pose = geometry_msgs.msg.PoseStamped()
cylinder2_pose.header.frame_id = self.robot.get_planning_frame()
cylinder2_pose.pose.position.x = 0.15
cylinder2_pose.pose.position.y = 0
cylinder2_pose.pose.position.z = 0.40
cylinder2_pose.pose.orientation.w = 1.0
cylinder2_pose.pose.orientation.x = 0.707 # 围绕x轴旋转90度（水平方向）
cylinder2_pose.pose.orientation.y = 0.0
cylinder2_pose.pose.orientation.z = 0.0
cylinder2_pose.pose.orientation.w = 0.707
self.scene.add_cylinder("cylinder2", cylinder2_pose, height=0.6, radius=0.02)
# 发布当前场景信息
planning_scene = moveit_msgs.msg.PlanningScene()
planning_scene.world.collision_objects.extend(self.scene.get_objects().values())
planning_scene.is_diff = True
planning_scene_publisher = rospy.Publisher('/planning_scene', moveit_msgs.msg.PlanningScene, queue_size=1)
planning_scene_publisher.publish(planning_scene)
rospy.sleep(2)
def robot_move(self):
# 控制机械臂回到初始化位置
self.arm_group.set_named_target('init_pose')
self.arm_group.go()
rospy.sleep(3)
# 设置机械臂运动的目标点，使用笛卡尔空间坐标位置表示（单位：米），姿态使用四元数表示
target_pose = geometry_msgs.msg.PoseStamped()
target_pose.header.frame_id = self.reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.142 # 设置目标点的x坐标
target_pose.pose.position.y = -0.140 # 设置目标点的y坐标
target_pose.pose.position.z = 0.075 # 设置目标点的z坐标
target_pose.pose.orientation.x = 0.026
target_pose.pose.orientation.y = 1.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 0.014
# 更新当前的位姿
self.arm_group.set_start_state_to_current_state()
# 获取机械臂当前的关节状态
current_joint_values = self.arm_group.get_current_joint_values()
# 打印当前关节状态
print("Current Joint Values:", current_joint_values)
print("end Joint Values:", self.end_effector_link)
# 设置机械臂的目标姿态
self.arm_group.set_pose_target(target_pose, self.end_effector_link)
# 进行运动规划
plan = self.arm_group.plan()
# print('plan point:', plan[1])
# 执行运动
self.arm_group.execute(plan[1])
rospy.sleep(3)
# 获取末端执行器的姿态
end_effector_pose = self.arm_group.get_current_pose().pose
# 打印末端执行器的坐标位置
print("End Effector Position:", end_effector_pose.position)
print("End Effector Orientation:", end_effector_pose.orientation)
# 控制机械臂末端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy
# self.arm_group.shift_pose_target(1, 0.22, self.end_effector_link)
# self.arm_group.go()
# rospy.sleep(5)
# 设置机械臂的目标位置，使用7轴的位置数据进行描述（单位：弧度）
# joint_pose = [0.2967, 0, 0, -1.57000, 0, -1.3439, 0]
# joint_pose = [0.2967, 0, 0, 0, 0, -1.3439, 0]
# arm_group.set_joint_value_target(joint_pose)
# 控制机械臂完成运动
# arm_group.go()
# rospy.sleep(10)
# 控制机械臂回到初始化位置
# arm_group.set_named_target('init_pose')
# arm_group.go()
def run(self):
# 移除所有障碍物
# self.scene.remove_world_object("cylinder1")
# self.scene.remove_world_object("cylinder2")
# 没有障碍物运行一次
# self.robot_move()
# 增加障碍物
self.add_scene()
rospy.sleep(3)
# 获取当前场景中的所有障碍物
current_obstacles = self.scene.get_known_object_names()
rospy.loginfo("Current obstacles in the scene: %s", current_obstacles)
rospy.sleep(2)
# 有障碍物后再运行一次
self.robot_move()
# 关闭MoveIt
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == '__main__':
try:
obstacle = MoveItPlanningDemo()
obstacle.run()
except rospy.ROSInterruptException:
pass

3
mycobot_280/mycobot_280/CMakeLists.txt
View file

@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS
scripts/teleop_keyboard.py
scripts/listen_real.py
scripts/listen_real_of_topic.py
scripts/detect_marker.py
scripts/following_marker.py
scripts/follow_and_pump.py
scripts/simple_gui.py
scripts/follow_display_gripper.py
scripts/slider_control_gripper.py

18
mycobot_280/mycobot_280/launch/detect_marker.launch
View file

@ -1,18 +0,0 @@
<launch>
<!-- Load file model ，加载文件模型-->
<arg name="model" default="$(find mycobot_description)/urdf/mycobot_280_m5/mycobot_280_m5.urdf"/>
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
<arg name="gui" default="true" />
<arg name="num" default="0" />
<include file="$(find mycobot_280)/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_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" />
</launch>

30
mycobot_280/mycobot_280/launch/detect_marker_with_topic.launch
View file

@ -1,30 +0,0 @@
<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_280_m5/mycobot_280_m5.urdf"/>
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
<arg name="gui" default="false" />
<arg name="num" default="0" />
<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" />
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- 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 ，监听并发布真实的角度-->
<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)"/>
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
</launch>

123
mycobot_280/mycobot_280/scripts/detect_marker.py
View file

@ -1,123 +0,0 @@
#!/usr/bin/env python
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in. 订阅者，监听是否有img
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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.如果没有，则计算相机矩阵
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# 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.处理标记数据
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose. 检测marker位姿。
# argument:
# marker corners,标记角
# marker size (meter),标记尺寸（米）
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker.只需选择第一个检测到的标记。
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# 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]，根据 [joint1] 变换姿势
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

194
mycobot_280/mycobot_280/scripts/follow_and_pump.py
View file

@ -1,194 +0,0 @@
#!/usr/bin/env python2
# coding:utf-8
import rospy
from visualization_msgs.msg import Marker
import time
import os
# 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)
# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
robot = os.popen("ls /dev/ttyUSB*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# instantiate the message object，实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# 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
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
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)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""Publish gripper status,发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
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)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
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
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# mark 信息的订阅者,subscribers to mark information
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

64
mycobot_280/mycobot_280/scripts/following_marker.py
View file

@ -1,64 +0,0 @@
#!/usr/bin/env python2
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/joint1"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial,标记位置初始化
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass

1
mycobot_280/mycobot_280_gripper_moveit/CMakeLists.txt
View file

@ -13,7 +13,6 @@ catkin_package()
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/sync_plan.py
scripts/path_planning_and_obstacle_avoidance_demo.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

137
mycobot_280/mycobot_280_gripper_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
View file

@ -1,137 +0,0 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, roslib, sys
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_euler
class MoveItPlanningDemo:
def __init__(self):
# API to initialize move_group,初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 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")
# 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
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.132
target_pose.pose.position.y = -0.150
target_pose.pose.position.z = 0.075
target_pose.pose.orientation.x = 0.026
target_pose.pose.orientation.y = 1.0
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()
rospy.sleep(1)
self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
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()
# rospy.sleep(1)
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()
suit_post.header.frame_id = self.reference_frame
suit_post.pose.position.x = 0.0
suit_post.pose.position.y = 0.0
suit_post.pose.position.z = -0.02
suit_post.pose.orientation.x = quat[0]
suit_post.pose.orientation.y = quat[1]
suit_post.pose.orientation.z = quat[2]
suit_post.pose.orientation.w = quat[3]
suit_path = (
roslib.packages.get_pkg_dir("mycobot_description")
+ "/urdf/mycobot/suit_env.dae"
)
# need `pyassimp==3.3`
self.scene.add_mesh("suit", suit_post, suit_path)
rospy.sleep(2)
# Run it again if there is an environmental impact,有环境影响后再运行一次
self.moving()
# close and exit moveit，关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == "__main__":
o = MoveItPlanningDemo()
o.run()

1
mycobot_280/mycobot_280_moveit/CMakeLists.txt
View file

@ -13,7 +13,6 @@ catkin_package()
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/sync_plan.py
scripts/path_planning_and_obstacle_avoidance_demo.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

137
mycobot_280/mycobot_280_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
View file

@ -1,137 +0,0 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, roslib, sys
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_euler
class MoveItPlanningDemo:
def __init__(self):
# API to initialize move_group,初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 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")
# 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
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.132
target_pose.pose.position.y = -0.150
target_pose.pose.position.z = 0.075
target_pose.pose.orientation.x = 0.026
target_pose.pose.orientation.y = 1.0
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()
rospy.sleep(1)
self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
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()
# rospy.sleep(1)
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()
suit_post.header.frame_id = self.reference_frame
suit_post.pose.position.x = 0.0
suit_post.pose.position.y = 0.0
suit_post.pose.position.z = -0.02
suit_post.pose.orientation.x = quat[0]
suit_post.pose.orientation.y = quat[1]
suit_post.pose.orientation.z = quat[2]
suit_post.pose.orientation.w = quat[3]
suit_path = (
roslib.packages.get_pkg_dir("mycobot_description")
+ "/urdf/mycobot/suit_env.dae"
)
# need `pyassimp==3.3`
self.scene.add_mesh("suit", suit_post, suit_path)
rospy.sleep(2)
# Run it again if there is an environmental impact,有环境影响后再运行一次
self.moving()
# close and exit moveit，关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == "__main__":
o = MoveItPlanningDemo()
o.run()

3
mycobot_280/mycobot_280arduino/CMakeLists.txt
View file

@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS
scripts/teleop_keyboard.py
scripts/listen_real.py
scripts/listen_real_of_topic.py
scripts/detect_marker.py
scripts/following_marker.py
scripts/follow_and_pump.py
scripts/simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

16
mycobot_280/mycobot_280arduino/launch/detect_marker.launch
View file

@ -1,16 +0,0 @@
<launch>
<arg name="model" default="$(find mycobot_description)/urdf/mycobot_280_arduino/mycobot_280_arduino.urdf"/>
<arg name="rvizconfig" default="$(find mycobot_280arduino)/config/mycobot_with_marker.rviz" />
<arg name="gui" default="true" />
<arg name="num" default="0" />
<include file="$(find mycobot_280arduino)/launch/slider_control.launch">
<arg name="model" value="$(arg model)" />
<arg name="rvizconfig" value="$(arg rvizconfig)" />
<arg name="gui" value="$(arg gui)" />
</include>
<node name="opencv_camera" pkg="mycobot_280arduino" type="opencv_camera" args="$(arg num)"/>
<node name="detect_marker" pkg="mycobot_280arduino" type="detect_marker.py" />
<node name="following_marker" pkg="mycobot_280arduino" type="following_marker.py" />
</launch>

29
mycobot_280/mycobot_280arduino/launch/detect_marker_with_topic.launch
View file

@ -1,29 +0,0 @@
<launch>
<arg name="port" default="/dev/ttyACM0" />
<arg name="baud" default="115200" />
<arg name="model" default="$(find mycobot_description)/urdf/mycobot_280_arduino/mycobot_280_arduino.urdf"/>
<arg name="rvizconfig" default="$(find mycobot_280arduino)/config/mycobot_with_marker.rviz" />
<arg name="gui" default="false" />
<arg name="num" default="0" />
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
<!-- Combinejoin values to 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 -->
<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 -->
<node name="real_listener" pkg="mycobot_280arduino" type="listen_real_of_topic.py" />
<!-- vision node -->
<node name="opencv_camera" pkg="mycobot_280arduino" type="opencv_camera" args="$(arg num)"/>
<node name="detect_marker" pkg="mycobot_280arduino" type="detect_marker.py" />
<node name="following_marker" pkg="mycobot_280arduino" type="following_marker.py" />
</launch>

123
mycobot_280/mycobot_280arduino/scripts/detect_marker.py
View file

@ -1,123 +0,0 @@
#!/usr/bin/env python
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in.
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv.
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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.
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# detect aruco marker.
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
corners, ids = ret[0], ret[1]
# process marker data.
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose.
# argument:
# marker corners
# marker size (meter)
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker.
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# get quaternion for 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]
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

190
mycobot_280/mycobot_280arduino/scripts/follow_and_pump.py
View file

@ -1,190 +0,0 @@
#!/usr/bin/env python2
# coding:utf-8
import rospy
from visualization_msgs.msg import Marker
import time
import os
# 与 mycobot 通信的消息类型
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
rospy.init_node("gipper_subscriber", anonymous=True)
# 控制 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
robot = os.popen("ls /dev/ttyUSB*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# 实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# 与 mycobot 真实位置的偏差值
x_offset = -20
y_offset = 20
z_offset = 110
# 通过该变量限制，抓取行为只做一次
flag = False
# 为了后面比较二维码是否移动
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):
"""发布坐标"""
coords.x = x
coords.y = y
coords.z = z
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
def pub_angles(a, b, c, d, e, f, sp):
"""发布角度"""
angles.joint_1 = float(a)
angles.joint_2 = float(b)
angles.joint_3 = float(c)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
def target_is_moving(x, y, z):
"""判断目标是否移动"""
count = 0
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
def grippercallback(data):
"""回调函数"""
global flag, temp_x, temp_y, temp_z
# rospy.loginfo('gripper_subscriber get date :%s', data)
if flag:
return
# 解析出坐标值
# 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"))
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
if (
time.time() - temp_time < 30
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: # 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# mark 信息的订阅者
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

64
mycobot_280/mycobot_280arduino/scripts/following_marker.py
View file

@ -1,64 +0,0 @@
#!/usr/bin/env python2
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/joint1"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass

1
mycobot_280/mycobot_280arduino_moveit/CMakeLists.txt
View file

@ -13,7 +13,6 @@ catkin_package()
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/sync_plan.py
scripts/path_planning_and_obstacle_avoidance_demo.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

125
mycobot_280/mycobot_280arduino_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
View file

@ -1,125 +0,0 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, roslib, sys
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_euler
class MoveItPlanningDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node("moveit_ik_demo")
# 初始化场景对象，用来监听外部环境的变化
self.scene = moveit_commander.PlanningSceneInterface()
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的self.arm group
self.arm = moveit_commander.MoveGroupCommander("arm_group")
# 获取终端link的名称
self.end_effector_link = self.arm.get_end_effector_link()
# 设置目标位置所使用的参考坐标系
self.reference_frame = "joint1"
self.arm.set_pose_reference_frame(self.reference_frame)
# 当运动规划失败后，允许重新规划
self.arm.allow_replanning(True)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.05)
def moving(self):
# # 控制机械臂先回到初始化位置
self.arm.set_named_target("init_pose")
self.arm.go()
rospy.sleep(2)
# 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
# 姿态使用四元数描述，基于base_link坐标系
target_pose = PoseStamped()
target_pose.header.frame_id = self.reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.132
target_pose.pose.position.y = -0.150
target_pose.pose.position.z = 0.075
target_pose.pose.orientation.x = 0.026
target_pose.pose.orientation.y = 1.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 0.014
# 设置机器臂当前的状态作为运动初始状态
self.arm.set_start_state_to_current_state()
# 设置机械臂终端运动的目标位姿
self.arm.set_pose_target(target_pose, self.end_effector_link)
# 规划运动路径
traj = self.arm.plan()
# 按照规划的运动路径控制机械臂运动
self.arm.execute(traj)
rospy.sleep(1)
# 控制机械臂终端向右移动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()
rospy.sleep(1)
self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
self.arm.go()
rospy.sleep(1)
# 控制机械臂终端反向旋转90度 0,1,2,3,4,5 代表xyzrpy
# self.arm.shift_pose_target(3, -1.57, end_effector_link)
# self.arm.go()
# rospy.sleep(1)
def run(self):
self.scene.remove_world_object("suit")
# 没有障碍物运行一次
self.moving()
# 添加环境
quat = quaternion_from_euler(3.1415, 0, -1.57)
suit_post = PoseStamped()
suit_post.header.frame_id = self.reference_frame
suit_post.pose.position.x = 0.0
suit_post.pose.position.y = 0.0
suit_post.pose.position.z = -0.02
suit_post.pose.orientation.x = quat[0]
suit_post.pose.orientation.y = quat[1]
suit_post.pose.orientation.z = quat[2]
suit_post.pose.orientation.w = quat[3]
suit_path = (
roslib.packages.get_pkg_dir("mycobot_description")
+ "/urdf/mycobot/suit_env.dae"
)
# need `pyassimp==3.3`
self.scene.add_mesh("suit", suit_post, suit_path)
rospy.sleep(2)
# 有环境影响后在运行一次
self.moving()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == "__main__":
o = MoveItPlanningDemo()
o.run()

3
mycobot_280/mycobot_280pi/CMakeLists.txt
View file

@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS
scripts/teleop_keyboard.py
scripts/listen_real.py
scripts/listen_real_of_topic.py
scripts/detect_marker.py
scripts/following_marker.py
scripts/follow_and_pump.py
scripts/simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

216
mycobot_280/mycobot_280pi/config/mycobot_with_marker.rviz
View file

@ -1,216 +0,0 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
- /TF1
- /Marker1
- /Marker1/Namespaces1
Splitter Ratio: 0.5
Tree Height: 775
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
joint1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint6_flange:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
basic_shapes:
Value: true
joint1:
Value: true
joint2:
Value: true
joint3:
Value: true
joint4:
Value: true
joint5:
Value: true
joint6:
Value: true
joint6_flange:
Value: true
Marker Scale: 0.300000012
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
joint1:
joint2:
joint3:
joint4:
joint5:
joint6:
joint6_flange:
basic_shapes:
{}
Update Interval: 0
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /visualization_marker
Name: Marker
Namespaces:
basic_cube: true
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: joint1
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.11990476
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.0706475973
Y: -0.0814988762
Z: 0.107583851
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.375398338
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.235389769
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1855
X: 65
Y: 24

18
mycobot_280/mycobot_280pi/launch/detect_marker.launch
View file

@ -1,18 +0,0 @@
<launch>
<!-- Load file model ，加载文件模型-->
<arg name="model" default="$(find mycobot_description)/urdf/mycobot_280_pi/mycobot_280_pi.urdf"/>
<arg name="rvizconfig" default="$(find mycobot_280pi)/config/mycobot_with_marker.rviz" />
<arg name="gui" default="true" />
<arg name="num" default="0" />
<include file="$(find mycobot_280pi)/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_280pi" type="opencv_camera" args="$(arg num)"/>
<node name="detect_marker" pkg="mycobot_280pi" type="detect_marker.py" />
<node name="following_marker" pkg="mycobot_280pi" type="following_marker.py" />
</launch>

30
mycobot_280/mycobot_280pi/launch/detect_marker_with_topic.launch
View file

@ -1,30 +0,0 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyAMA0" />
<arg name="baud" default="1000000" />
<!-- Load file model ，加载文件模型-->
<arg name="model" default="$(find mycobot_description)/urdf/mycobot_280_pi/mycobot_280_pi.urdf"/>
<arg name="rvizconfig" default="$(find mycobot_280pi)/config/mycobot_with_marker.rviz" />
<arg name="gui" default="false" />
<arg name="num" default="0" />
<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" />
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- 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 ，监听并发布真实的角度-->
<node name="real_listener" pkg="mycobot_280pi" type="listen_real_of_topic.py" />
<!-- vision node -->
<node name="opencv_camera" pkg="mycobot_280pi" type="opencv_camera" args="$(arg num)"/>
<node name="detect_marker" pkg="mycobot_280pi" type="detect_marker.py" />
<node name="following_marker" pkg="mycobot_280pi" type="following_marker.py" />
</launch>

124
mycobot_280/mycobot_280pi/scripts/detect_marker.py
View file

@ -1,124 +0,0 @@
#!/usr/bin/env python
# encoding=utf-8
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in. 订阅者，监听是否有img
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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.如果没有，则计算相机矩阵
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# 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.处理标记数据
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose. 检测marker位姿。
# argument:
# marker corners,标记角
# marker size (meter),标记尺寸（米）
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker.只需选择第一个检测到的标记。
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# 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]，根据 [joint1] 变换姿势
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

194
mycobot_280/mycobot_280pi/scripts/follow_and_pump.py
View file

@ -1,194 +0,0 @@
#!/usr/bin/env python2
# coding:utf-8
import rospy
from visualization_msgs.msg import Marker
import time
import os
# 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)
# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
robot = os.popen("ls /dev/ttyAMA*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# instantiate the message object，实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# 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
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
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)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""Publish gripper status,发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
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)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
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
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# mark 信息的订阅者,subscribers to mark information
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

65
mycobot_280/mycobot_280pi/scripts/following_marker.py
View file

@ -1,65 +0,0 @@
#!/usr/bin/env python2
# encoding=utf-8
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/joint1"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial,标记位置初始化
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass

1
mycobot_280/mycobot_280pi_moveit/CMakeLists.txt
View file

@ -13,7 +13,6 @@ catkin_package()
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/sync_plan.py
scripts/path_planning_and_obstacle_avoidance_demo.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

137
mycobot_280/mycobot_280pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
View file

@ -1,137 +0,0 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, roslib, sys
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_euler
class MoveItPlanningDemo:
def __init__(self):
# API to initialize move_group,初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 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")
# 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
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.132
target_pose.pose.position.y = -0.150
target_pose.pose.position.z = 0.075
target_pose.pose.orientation.x = 0.026
target_pose.pose.orientation.y = 1.0
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()
rospy.sleep(1)
self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
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()
# rospy.sleep(1)
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()
suit_post.header.frame_id = self.reference_frame
suit_post.pose.position.x = 0.0
suit_post.pose.position.y = 0.0
suit_post.pose.position.z = -0.02
suit_post.pose.orientation.x = quat[0]
suit_post.pose.orientation.y = quat[1]
suit_post.pose.orientation.z = quat[2]
suit_post.pose.orientation.w = quat[3]
suit_path = (
roslib.packages.get_pkg_dir("mycobot_description")
+ "/urdf/mycobot/suit_env.dae"
)
# need `pyassimp==3.3`
self.scene.add_mesh("suit", suit_post, suit_path)
rospy.sleep(2)
# Run it again if there is an environmental impact,有环境影响后再运行一次
self.moving()
# close and exit moveit，关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == "__main__":
o = MoveItPlanningDemo()
o.run()

3
mycobot_320/mycobot_320/CMakeLists.txt
View file

@ -36,9 +36,6 @@ catkin_install_python(PROGRAMS
scripts/mycobot_320_teleop_keyboard.py
scripts/mycobot_320_listen_real.py
scripts/mycobot_320_listen_real_of_topic.py
scripts/mycobot_320_detect_marker.py
scripts/mycobot_320_following_marker.py
scripts/mycobot_320_follow_and_pump.py
scripts/mycobot_320_simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

16
mycobot_320/mycobot_320/launch/mycobot_320_detect_marker.launch
View file

@ -1,16 +0,0 @@
<launch>
<arg name="model" default="$(find mycobot_description)/urdf/mycobot_320_m5_2020/mycobot_pro_320_m5_2020.urdf"/>
<arg name="rvizconfig" default="$(find mycobot_320)/config/mycobot_320_with_marker.rviz" />
<arg name="gui" default="true" />
<arg name="num" default="0" />
<include file="$(find mycobot_320)/launch/mycobot_320_slider.launch">
<arg name="model" value="$(arg model)" />
<arg name="rvizconfig" value="$(arg rvizconfig)" />
<arg name="gui" value="$(arg gui)" />
</include>
<node name="opencv_camera" pkg="mycobot_320" type="opencv_camera" args="$(arg num)"/>
<node name="detect_marker" pkg="mycobot_320" type="mycobot_320_detect_marker.py" />
<node name="following_marker" pkg="mycobot_320" type="mycobot_320_following_marker.py" />
</launch>

29
mycobot_320/mycobot_320/launch/mycobot_320_detect_marker_with_topic.launch
View file

@ -1,29 +0,0 @@
<launch>
<arg name="port" default="/dev/ttyUSB0" />
<arg name="baud" default="115200" />
<arg name="model" default="$(find mycobot_description)/urdf/mycobot_320_m5_2020/mycobot_pro_320_m5_2020.urdf"/>
<arg name="rvizconfig" default="$(find mycobot_320)/config/mycobot_320_with_marker.rviz" />
<arg name="gui" default="false" />
<arg name="num" default="0" />
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
<!-- Combinejoin values to 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 -->
<include file="$(find mycobot_320_communication)/launch/communication_topic.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_320" type="mycobot_320_listen_real_of_topic.py" />
<!-- vision node -->
<node name="opencv_camera" pkg="mycobot_320" type="opencv_camera" args="$(arg num)"/>
<node name="detect_marker" pkg="mycobot_320" type="mycobot_320_detect_marker.py" />
<node name="following_marker" pkg="mycobot_320" type="mycobot_320_following_marker.py" />
</launch>

124
mycobot_320/mycobot_320/scripts/mycobot_320_detect_marker.py
View file

@ -1,124 +0,0 @@
#!/usr/bin/env python
# -*- coding:utf-8 -*-
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in.
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv.
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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.
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# detect aruco marker.
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
corners, ids = ret[0], ret[1]
# process marker data.
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose.
# argument:
# marker corners
# marker size (meter)
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker.
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# get quaternion for 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]
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

190
mycobot_320/mycobot_320/scripts/mycobot_320_follow_and_pump.py
View file

@ -1,190 +0,0 @@
#!/usr/bin/env python2
# -*- coding:utf-8 -*-
import rospy
from visualization_msgs.msg import Marker
import time
import os
# 与 mycobot 通信的消息类型
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
rospy.init_node("gipper_subscriber", anonymous=True)
# 控制 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
robot = os.popen("ls /dev/ttyUSB*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# 实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# 与 mycobot 真实位置的偏差值
x_offset = -20
y_offset = 20
z_offset = 110
# 通过该变量限制，抓取行为只做一次
flag = False
# 为了后面比较二维码是否移动
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):
"""发布坐标"""
coords.x = x
coords.y = y
coords.z = z
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
def pub_angles(a, b, c, d, e, f, sp):
"""发布角度"""
angles.joint_1 = float(a)
angles.joint_2 = float(b)
angles.joint_3 = float(c)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
def target_is_moving(x, y, z):
"""判断目标是否移动"""
count = 0
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
def grippercallback(data):
"""回调函数"""
global flag, temp_x, temp_y, temp_z
# rospy.loginfo('gripper_subscriber get date :%s', data)
if flag:
return
# 解析出坐标值
# 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"))
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
if (
time.time() - temp_time < 30
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: # 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# mark 信息的订阅者
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

65
mycobot_320/mycobot_320/scripts/mycobot_320_following_marker.py
View file

@ -1,65 +0,0 @@
#!/usr/bin/env python2
# -*- coding:utf-8 -*-
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/joint1"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass

3
mycobot_320/new_mycobot_320/CMakeLists.txt
View file

@ -70,9 +70,6 @@ catkin_install_python(PROGRAMS
scripts/mycobot_320_teleop_keyboard.py
scripts/mycobot_320_listen_real.py
scripts/mycobot_320_listen_real_of_topic.py
scripts/mycobot_320_detect_marker.py
scripts/mycobot_320_following_marker.py
scripts/mycobot_320_follow_and_pump.py
scripts/mycobot_320_simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

16
mycobot_320/new_mycobot_320/launch/mycobot_320_detect_marker.launch
View file

@ -1,16 +0,0 @@
<launch>
<arg name="model" default="$(find mycobot_description)/urdf/mycobot_320_m5_2022/new_mycobot_pro_320_m5_2022.urdf"/>
<arg name="rvizconfig" default="$(find new_mycobot_320)/config/mycobot_320_with_marker.rviz" />
<arg name="gui" default="true" />
<arg name="num" default="0" />
<include file="$(find new_mycobot_320)/launch/mycobot_320_slider.launch">
<arg name="model" value="$(arg model)" />
<arg name="rvizconfig" value="$(arg rvizconfig)" />
<arg name="gui" value="$(arg gui)" />
</include>
<node name="opencv_camera" pkg="new_mycobot_320" type="opencv_camera" args="$(arg num)"/>
<node name="detect_marker" pkg="new_mycobot_320" type="mycobot_320_detect_marker.py" />
<node name="following_marker" pkg="new_mycobot_320" type="mycobot_320_following_marker.py" />
</launch>

29
mycobot_320/new_mycobot_320/launch/mycobot_320_detect_marker_with_topic.launch
View file

@ -1,29 +0,0 @@
<launch>
<arg name="port" default="/dev/ttyUSB0" />
<arg name="baud" default="115200" />
<arg name="model" default="$(find mycobot_description)/urdf/mycobot_320_m5_2022/new_mycobot_pro_320_m5_2022.urdf"/>
<arg name="rvizconfig" default="$(find new_mycobot_320)/config/mycobot_320_with_marker.rviz" />
<arg name="gui" default="false" />
<arg name="num" default="0" />
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
<!-- Combinejoin values to 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 -->
<include file="$(find mycobot_320_communication)/launch/communication_topic.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="new_mycobot_320" type="mycobot_320_listen_real_of_topic.py" />
<!-- vision node -->
<node name="opencv_camera" pkg="new_mycobot_320" type="opencv_camera" args="$(arg num)"/>
<node name="detect_marker" pkg="new_mycobot_320" type="mycobot_320_detect_marker.py" />
<node name="following_marker" pkg="new_mycobot_320" type="mycobot_320_following_marker.py" />
</launch>

124
mycobot_320/new_mycobot_320/scripts/mycobot_320_detect_marker.py
View file

@ -1,124 +0,0 @@
#!/usr/bin/env python
# -*- coding:utf-8 -*-
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in.
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv.
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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.
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# detect aruco marker.
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
corners, ids = ret[0], ret[1]
# process marker data.
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose.
# argument:
# marker corners
# marker size (meter)
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker.
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# get quaternion for 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]
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

190
mycobot_320/new_mycobot_320/scripts/mycobot_320_follow_and_pump.py
View file

@ -1,190 +0,0 @@
#!/usr/bin/env python2
# -*- coding:utf-8 -*-
import rospy
from visualization_msgs.msg import Marker
import time
import os
# 与 mycobot 通信的消息类型
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
rospy.init_node("gipper_subscriber", anonymous=True)
# 控制 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
robot = os.popen("ls /dev/ttyUSB*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# 实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# 与 mycobot 真实位置的偏差值
x_offset = -20
y_offset = 20
z_offset = 110
# 通过该变量限制，抓取行为只做一次
flag = False
# 为了后面比较二维码是否移动
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):
"""发布坐标"""
coords.x = x
coords.y = y
coords.z = z
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
def pub_angles(a, b, c, d, e, f, sp):
"""发布角度"""
angles.joint_1 = float(a)
angles.joint_2 = float(b)
angles.joint_3 = float(c)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
def target_is_moving(x, y, z):
"""判断目标是否移动"""
count = 0
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
def grippercallback(data):
"""回调函数"""
global flag, temp_x, temp_y, temp_z
# rospy.loginfo('gripper_subscriber get date :%s', data)
if flag:
return
# 解析出坐标值
# 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"))
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
if (
time.time() - temp_time < 30
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: # 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# mark 信息的订阅者
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

65
mycobot_320/new_mycobot_320/scripts/mycobot_320_following_marker.py
View file

@ -1,65 +0,0 @@
#!/usr/bin/env python2
# -*- coding:utf-8 -*-
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/joint1"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass

137
mycobot_320/new_mycobot_320_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
View file

@ -1,137 +0,0 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, roslib, sys
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_euler
class MoveItPlanningDemo:
def __init__(self):
# API to initialize move_group,初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 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")
# 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
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.132
target_pose.pose.position.y = -0.150
target_pose.pose.position.z = 0.075
target_pose.pose.orientation.x = 0.026
target_pose.pose.orientation.y = 1.0
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()
rospy.sleep(1)
self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
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()
# rospy.sleep(1)
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()
suit_post.header.frame_id = self.reference_frame
suit_post.pose.position.x = 0.0
suit_post.pose.position.y = 0.0
suit_post.pose.position.z = -0.02
suit_post.pose.orientation.x = quat[0]
suit_post.pose.orientation.y = quat[1]
suit_post.pose.orientation.z = quat[2]
suit_post.pose.orientation.w = quat[3]
suit_path = (
roslib.packages.get_pkg_dir("mycobot_description")
+ "/urdf/mycobot/suit_env.dae"
)
# need `pyassimp==3.3`
self.scene.add_mesh("suit", suit_post, suit_path)
rospy.sleep(2)
# Run it again if there is an environmental impact,有环境影响后再运行一次
self.moving()
# close and exit moveit，关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == "__main__":
o = MoveItPlanningDemo()
o.run()

3
mycobot_320/new_mycobot_320_pi/CMakeLists.txt
View file

@ -71,9 +71,6 @@ catkin_install_python(PROGRAMS
scripts/mycobot_320_teleop_keyboard.py
scripts/mycobot_320_listen_real.py
scripts/mycobot_320_listen_real_of_topic.py
scripts/mycobot_320_detect_marker.py
scripts/mycobot_320_following_marker.py
scripts/mycobot_320_follow_and_pump.py
scripts/mycobot_320_simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

216
mycobot_320/new_mycobot_320_pi/config/mycobot_320_with_marker.rviz
View file

@ -1,216 +0,0 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
- /TF1
- /Marker1
- /Marker1/Namespaces1
Splitter Ratio: 0.5
Tree Height: 662
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
joint1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint6_flange:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
basic_shapes:
Value: true
joint1:
Value: true
joint2:
Value: true
joint3:
Value: true
joint4:
Value: true
joint5:
Value: true
joint6:
Value: true
joint6_flange:
Value: true
Marker Scale: 0.300000012
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
joint1:
joint2:
joint3:
joint4:
joint5:
joint6:
joint6_flange:
basic_shapes:
{}
Update Interval: 0
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /visualization_marker
Name: Marker
Namespaces:
basic_cube: true
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: base
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1.20289087
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.0706475973
Y: -0.0814988762
Z: 0.107583851
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.440398335
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.430389732
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 903
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1853
X: 65
Y: 24

16
mycobot_320/new_mycobot_320_pi/launch/mycobot_320_detect_marker.launch
View file

@ -1,16 +0,0 @@
<launch>
<arg name="model" default="$(find mycobot_description)/urdf/mycobot_320_pi_2022/new_mycobot_pro_320_pi_2022.urdf"/>
<arg name="rvizconfig" default="$(find new_mycobot_320_pi)/config/mycobot_320_with_marker.rviz" />
<arg name="gui" default="true" />
<arg name="num" default="0" />
<include file="$(find new_mycobot_320_pi)/launch/mycobot_320_slider.launch">
<arg name="model" value="$(arg model)" />
<arg name="rvizconfig" value="$(arg rvizconfig)" />
<arg name="gui" value="$(arg gui)" />
</include>
<node name="opencv_camera" pkg="new_mycobot_320_pi" type="opencv_camera" args="$(arg num)"/>
<node name="detect_marker" pkg="new_mycobot_320_pi" type="mycobot_320_detect_marker.py" />
<node name="following_marker" pkg="new_mycobot_320_pi" type="mycobot_320_following_marker.py" />
</launch>

29
mycobot_320/new_mycobot_320_pi/launch/mycobot_320_detect_marker_with_topic.launch
View file

@ -1,29 +0,0 @@
<launch>
<arg name="port" default="/dev/ttyAMA0" />
<arg name="baud" default="1000000" />
<arg name="model" default="$(find mycobot_description)/urdf/mycobot_320_pi_2022/new_mycobot_pro_320_pi_2022.urdf"/>
<arg name="rvizconfig" default="$(find new_mycobot_320_pi)/config/mycobot_320_with_marker.rviz" />
<arg name="gui" default="false" />
<arg name="num" default="0" />
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
<!-- Combinejoin values to 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 -->
<include file="$(find mycobot_320_communication)/launch/communication_topic.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="new_mycobot_320_pi" type="mycobot_320_listen_real_of_topic.py" />
<!-- vision node -->
<node name="opencv_camera" pkg="new_mycobot_320_pi" type="opencv_camera" args="$(arg num)"/>
<node name="detect_marker" pkg="new_mycobot_320_pi" type="mycobot_320_detect_marker.py" />
<node name="following_marker" pkg="new_mycobot_320_pi" type="mycobot_320_following_marker.py" />
</launch>

124
mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_detect_marker.py
View file

@ -1,124 +0,0 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in.
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv.
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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.
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# detect aruco marker.
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
corners, ids = ret[0], ret[1]
# process marker data.
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose.
# argument:
# marker corners
# marker size (meter)
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker.
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# get quaternion for 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]
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

190
mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_follow_and_pump.py
View file

@ -1,190 +0,0 @@
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
import rospy
from visualization_msgs.msg import Marker
import time
import os
# 与 mycobot 通信的消息类型
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
rospy.init_node("gipper_subscriber", anonymous=True)
# 控制 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
robot = os.popen("ls /dev/ttyUSB*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# 实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# 与 mycobot 真实位置的偏差值
x_offset = -20
y_offset = 20
z_offset = 110
# 通过该变量限制，抓取行为只做一次
flag = False
# 为了后面比较二维码是否移动
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):
"""发布坐标"""
coords.x = x
coords.y = y
coords.z = z
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
def pub_angles(a, b, c, d, e, f, sp):
"""发布角度"""
angles.joint_1 = float(a)
angles.joint_2 = float(b)
angles.joint_3 = float(c)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
def target_is_moving(x, y, z):
"""判断目标是否移动"""
count = 0
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
def grippercallback(data):
"""回调函数"""
global flag, temp_x, temp_y, temp_z
# rospy.loginfo('gripper_subscriber get date :%s', data)
if flag:
return
# 解析出坐标值
# 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"))
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
if (
time.time() - temp_time < 30
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: # 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# mark 信息的订阅者
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

65
mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_following_marker.py
View file

@ -1,65 +0,0 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/joint1"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass

137
mycobot_320/new_mycobot_320_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
View file

@ -1,137 +0,0 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, roslib, sys
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_euler
class MoveItPlanningDemo:
def __init__(self):
# API to initialize move_group,初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 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")
# 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
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.132
target_pose.pose.position.y = -0.150
target_pose.pose.position.z = 0.075
target_pose.pose.orientation.x = 0.026
target_pose.pose.orientation.y = 1.0
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()
rospy.sleep(1)
self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
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()
# rospy.sleep(1)
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()
suit_post.header.frame_id = self.reference_frame
suit_post.pose.position.x = 0.0
suit_post.pose.position.y = 0.0
suit_post.pose.position.z = -0.02
suit_post.pose.orientation.x = quat[0]
suit_post.pose.orientation.y = quat[1]
suit_post.pose.orientation.z = quat[2]
suit_post.pose.orientation.w = quat[3]
suit_path = (
roslib.packages.get_pkg_dir("mycobot_description")
+ "/urdf/mycobot/suit_env.dae"
)
# need `pyassimp==3.3`
self.scene.add_mesh("suit", suit_post, suit_path)
rospy.sleep(2)
# Run it again if there is an environmental impact,有环境影响后再运行一次
self.moving()
# close and exit moveit，关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == "__main__":
o = MoveItPlanningDemo()
o.run()

3
mypalletizer_260/mypalletizer_260/CMakeLists.txt
View file

@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS
scripts/teleop_keyboard.py
scripts/listen_real.py
scripts/listen_real_of_topic.py
scripts/detect_marker.py
scripts/following_marker.py
scripts/follow_and_pump.py
scripts/simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

216
mypalletizer_260/mypalletizer_260/config/mycobot_with_marker.rviz
View file

@ -1,216 +0,0 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
- /TF1
- /Marker1
- /Marker1/Namespaces1
Splitter Ratio: 0.5
Tree Height: 775
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
joint1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint6_flange:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
basic_shapes:
Value: true
joint1:
Value: true
joint2:
Value: true
joint3:
Value: true
joint4:
Value: true
joint5:
Value: true
joint6:
Value: true
joint6_flange:
Value: true
Marker Scale: 0.300000012
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
joint1:
joint2:
joint3:
joint4:
joint5:
joint6:
joint6_flange:
basic_shapes:
{}
Update Interval: 0
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /visualization_marker
Name: Marker
Namespaces:
basic_cube: true
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: joint1
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.11990476
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.0706475973
Y: -0.0814988762
Z: 0.107583851
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.375398338
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.235389769
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1855
X: 65
Y: 24

18
mypalletizer_260/mypalletizer_260/launch/detect_marker.launch
View file

@ -1,18 +0,0 @@
<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" />
<arg name="num" default="0" />
<include file="$(find mycobot_280)/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_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" />
</launch>

31
mypalletizer_260/mypalletizer_260/launch/detect_marker_with_topic.launch
View file

@ -1,31 +0,0 @@
<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" />
<arg name="num" default="0" />
<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" />
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- 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，坚听并发布真实的角度 -->
<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)"/>
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
</launch>

124
mypalletizer_260/mypalletizer_260/scripts/detect_marker.py
View file

@ -1,124 +0,0 @@
#!/usr/bin/env python
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in. 订阅者，监听是否有img进来
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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. 如果没有就计算相机矩阵
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# 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.
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose. 检测标记姿势
# argument:
# marker corners, 标记角
# marker size (meter), 标记尺寸（米）
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker. 只需选择第一个检测到的标记
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# 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]
# 根据 [joint1] 变换姿势
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

197
mypalletizer_260/mypalletizer_260/scripts/follow_and_pump.py
View file

@ -1,197 +0,0 @@
#!/usr/bin/env python2
# coding:utf-8
import rospy
from visualization_msgs.msg import Marker
import time
import os
# Type of message to communicate 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的话题，依次是角度、坐标、夹爪
angle_pub = rospy.Publisher("mycobot/angles_goal",
MycobotSetAngles, queue_size=5)
coord_pub = rospy.Publisher("mycobot/coords_goal",
MycobotSetCoords, queue_size=5)
# Judging device: ttyUSB* is M5；ttyACM* is wio
# 判断设备：ttyUSB*为M5；ttyACM*为wio
robot = os.popen("ls /dev/ttyUSB*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# Instantiate the message object. 实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# Deviation value from the real position of mycobot
# 与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
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
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)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""Publish gripper status. 发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
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)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
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
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: # Indicates that the target is in a stationary state and can be attempted to grab. 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# subscribers to mark information, mark信息的订阅者
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

64
mypalletizer_260/mypalletizer_260/scripts/following_marker.py
View file

@ -1,64 +0,0 @@
#!/usr/bin/env python2
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/joint1"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker 标记
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial. 标记初始位置
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass

3
mypalletizer_260/mypalletizer_260_pi/CMakeLists.txt
View file

@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS
scripts/teleop_keyboard.py
scripts/listen_real.py
scripts/listen_real_of_topic.py
scripts/detect_marker.py
scripts/following_marker.py
scripts/follow_and_pump.py
scripts/simple_gui.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

216
mypalletizer_260/mypalletizer_260_pi/config/mycobot_with_marker.rviz
View file

@ -1,216 +0,0 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
- /TF1
- /Marker1
- /Marker1/Namespaces1
Splitter Ratio: 0.5
Tree Height: 775
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
joint1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
joint6_flange:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
basic_shapes:
Value: true
joint1:
Value: true
joint2:
Value: true
joint3:
Value: true
joint4:
Value: true
joint5:
Value: true
joint6:
Value: true
joint6_flange:
Value: true
Marker Scale: 0.300000012
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
joint1:
joint2:
joint3:
joint4:
joint5:
joint6:
joint6_flange:
basic_shapes:
{}
Update Interval: 0
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /visualization_marker
Name: Marker
Namespaces:
basic_cube: true
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: joint1
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.11990476
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.0706475973
Y: -0.0814988762
Z: 0.107583851
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.375398338
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.235389769
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1855
X: 65
Y: 24

18
mypalletizer_260/mypalletizer_260_pi/launch/detect_marker.launch
View file

@ -1,18 +0,0 @@
<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" />
<arg name="num" default="0" />
<include file="$(find mycobot_280)/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_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" />
</launch>

31
mypalletizer_260/mypalletizer_260_pi/launch/detect_marker_with_topic.launch
View file

@ -1,31 +0,0 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyAMA0" />
<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" />
<arg name="num" default="0" />
<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" />
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- 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，坚听并发布真实的角度 -->
<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)"/>
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
</launch>

125
mypalletizer_260/mypalletizer_260_pi/scripts/detect_marker.py
View file

@ -1,125 +0,0 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import tf
from tf.broadcaster import TransformBroadcaster
import tf_conversions
from mycobot_communication.srv import (
GetCoords,
SetCoords,
GetAngles,
SetAngles,
GripperStatus,
)
class ImageConverter:
def __init__(self):
self.br = TransformBroadcaster()
self.bridge = CvBridge()
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
self.aruo_params = cv.aruco.DetectorParameters_create()
calibrationParams = cv.FileStorage(
"calibrationFileName.xml", cv.FILE_STORAGE_READ
)
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
self.camera_matrix = None
# subscriber, listen wether has img come in. 订阅者，监听是否有img进来
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
def callback(self, data):
"""Callback function.
Process image with OpenCV, detect Mark to get the pose. Then acccording the
pose to transforming.
"""
try:
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
size = cv_image.shape
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. 如果没有就计算相机矩阵
self.camera_matrix = np.array(
[
[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1],
],
dtype=np.float32,
)
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
# 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.
if len(corners) > 0:
if ids is not None:
# print('corners:', corners, 'ids:', ids)
# detect marker pose. 检测标记姿势
# argument:
# marker corners, 标记角
# marker size (meter), 标记尺寸（米）
ret = cv.aruco.estimatePoseSingleMarkers(
corners, 0.05, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
(rvec - tvec).any()
print("rvec:", rvec, "tvec:", tvec)
# just select first one detected marker. 只需选择第一个检测到的标记
for i in range(rvec.shape[0]):
cv.aruco.drawDetectedMarkers(cv_image, corners)
cv.aruco.drawAxis(
cv_image,
self.camera_matrix,
self.dist_coeffs,
rvec[i, :, :],
tvec[i, :, :],
0.03,
)
xyz = tvec[0, 0, :]
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
# 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]
# 根据 [joint1] 变换姿势
self.br.sendTransform(
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
)
# [x, y, z, -172, 3, -46.8]
cv.imshow("Image", cv_image)
cv.waitKey(3)
try:
pass
except CvBridgeError as e:
print(e)
if __name__ == "__main__":
try:
rospy.init_node("detect_marker")
rospy.loginfo("Starting cv_bridge_test node")
ImageConverter()
rospy.spin()
except KeyboardInterrupt:
print("Shutting down cv_bridge_test node.")
cv.destroyAllWindows()

197
mypalletizer_260/mypalletizer_260_pi/scripts/follow_and_pump.py
View file

@ -1,197 +0,0 @@
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
import rospy
from visualization_msgs.msg import Marker
import time
import os
# Type of message to communicate 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的话题，依次是角度、坐标、夹爪
angle_pub = rospy.Publisher("mycobot/angles_goal",
MycobotSetAngles, queue_size=5)
coord_pub = rospy.Publisher("mycobot/coords_goal",
MycobotSetCoords, queue_size=5)
# Judging device: ttyUSB* is M5；ttyACM* is wio
# 判断设备：ttyUSB*为M5；ttyACM*为wio
robot = os.popen("ls /dev/ttyUSB*").readline()
if "dev" in robot:
Pin = [2, 5]
else:
Pin = [20, 21]
pump_pub = rospy.Publisher("mycobot/pump_status",
MycobotPumpStatus, queue_size=5)
# Instantiate the message object. 实例化消息对象
angles = MycobotSetAngles()
coords = MycobotSetCoords()
pump = MycobotPumpStatus()
# Deviation value from the real position of mycobot
# 与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
coords.rx = rx
coords.ry = ry
coords.rz = rz
coords.speed = 70
coords.model = m
# print(coords)
coord_pub.publish(coords)
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)
angles.joint_4 = float(d)
angles.joint_5 = float(e)
angles.joint_6 = float(f)
angles.speed = sp
angle_pub.publish(angles)
def pub_pump(flag, Pin):
"""Publish gripper status. 发布夹爪状态"""
pump.Status = flag
pump.Pin1 = Pin[0]
pump.Pin2 = Pin[1]
pump_pub.publish(pump)
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)):
print(o, n)
if abs(o - n) < 2:
count += 1
print(count)
if count == 3:
return False
return True
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
or (temp_x == temp_y == temp_z == 0.0)
or target_is_moving(x - x_offset, y - y_offset, z)
):
x -= x_offset
y -= y_offset
pub_coords(x - 20, y, 280)
time.sleep(0.1)
temp_x, temp_y, temp_z = x, y, z
return
else: # Indicates that the target is in a stationary state and can be attempted to grab. 表示目标处于静止状态，可以尝试抓取
print(x, y, z)
# detect heigth + pump height + limit height + offset
x += x_offset
y += y_offset
z = z + 88 + z_offset
pub_coords(x, y, z)
time.sleep(2.5)
# down
for i in range(1, 17):
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
time.sleep(0.1)
time.sleep(2)
pub_pump(True, Pin)
# pump on
pub_coords(x, y, z + 20, -165)
time.sleep(1.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
put_z = 140
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
time.sleep(1.5)
for i in range(1, 8):
pub_coords(39.4, -174.7, put_z - i * 5, -
177.13, -4.13, -152.59, 15, 2)
time.sleep(0.1)
pub_pump(False, Pin)
time.sleep(0.5)
pub_angles(0, 30, -50, -40, 0, 0, 50)
time.sleep(1.5)
# finally
flag = True
def main():
for _ in range(10):
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
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)
time.sleep(0.5)
pub_pump(False, Pin)
# time.sleep(2.5)
# subscribers to mark information, mark信息的订阅者
rospy.Subscriber("visualization_marker", Marker,
grippercallback, queue_size=1)
print("gripper test")
rospy.spin()
if __name__ == "__main__":
main()

65
mypalletizer_260/mypalletizer_260_pi/scripts/following_marker.py
View file

@ -1,65 +0,0 @@
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
import time
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf
def talker():
rospy.init_node("following_marker", anonymous=True)
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rate = rospy.Rate(20)
listener = tf.TransformListener()
marker_ = Marker()
marker_.header.frame_id = "/joint1"
marker_.ns = "basic_cube"
print("publishing ...")
while not rospy.is_shutdown():
now = rospy.Time.now() - rospy.Duration(0.1)
try:
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
except Exception as e:
print(e)
continue
print(type(trans), trans)
print(type(rot), rot)
# marker 标记
marker_.header.stamp = now
marker_.type = marker_.CUBE
marker_.action = marker_.ADD
marker_.scale.x = 0.04
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial. 标记初始位置
marker_.pose.position.x = trans[0]
marker_.pose.position.y = trans[1]
marker_.pose.position.z = trans[2]
marker_.pose.orientation.x = rot[0]
marker_.pose.orientation.y = rot[1]
marker_.pose.orientation.z = rot[2]
marker_.pose.orientation.w = rot[3]
marker_.color.a = 1.0
marker_.color.g = 1.0
pub_marker.publish(marker_)
rate.sleep()
if __name__ == "__main__":
try:
talker()
except rospy.ROSInterruptException:
pass