快速使用
- 新建终端
1
roscore
- 终端2
1
python3 amov_gimbal_image_pub.py
- 终端3
1
python2 aruco_detecter.py
- 终端4
1 2
sudo chmod 777 /dev/tty* python3 amov_gimbal_move.py
- 该实验所用的是ID_582的Aruco码，如需更换请更改aruco_detecter.py相应代码行
amov_gimbal_image_pub.py
网络摄像头获取图像并通过ROS话题发布
Pub：/image_view/image_raw
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
Syntax highlighted code block
import cv2
import numpy as np
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image
from cv_bridge import CvBridge , CvBridgeError
import time
if __name__=="__main__":
pipeline = "rtspsrc location=rtsp://192.168.2.64:/H264?W=720&H=480&FPS=30&BR=10000000 latency=100 \
caps='application/x-rtp,media=(string)video,clock-rate=(int)90000,encoding-name=\
(string)H264,width=1920,height=1080,framerate=30/1' !\
rtph264depay ! h264parse ! omxh264dec ! nvvidconv ! \
video/x-raw, width=(int)720, height=(int)480, format=(string)BGRx ! \
videoconvert ! appsink sync=false"
capture = cv2.VideoCapture(pipeline)
rospy.init_node('camera_node', anonymous=True) #定义节点
image_pub=rospy.Publisher('/image_view/image_raw', Image, queue_size = 100) #定义话题
while not rospy.is_shutdown(): # Ctrl C正常退出，如果异常退出会报错device busy！
start = time.time()
ret, frame = capture.read()
if ret: # 如果有画面再执行
# frame = cv2.flip(frame,0) #垂直镜像操作
#frame = cv2.flip(frame,1) #水平镜像操作
ros_frame = Image()
header = Header(stamp = rospy.Time.now())
header.frame_id = "Camera"
ros_frame.header=header
ros_frame.width = 720
ros_frame.height = 480
ros_frame.encoding = "bgr8"
ros_frame.step = 1920
ros_frame.data = np.array(frame).tostring() #图片格式转换
image_pub.publish(ros_frame) #发布消息
end = time.time()
print("cost time:", end-start ) # 看一下每一帧的执行时间，从而确定合适的rate
rate = rospy.Rate(20) # 10hz
capture.release()
cv2.destroyAllWindows()
print("quit successfully!")
aruco_detecter.py
订阅视频流话题，获取标识相对位姿并通过ROS话题发布
Sub：/image_view/image_raw
Pub：aruco_offset
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
Syntax highlighted code block
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from move_base_msgs.msg import MoveBaseActionResult
from geometry_msgs.msg import Pose, Point, Quaternion, PoseStamped
from sensor_msgs.msg import Image
from std_msgs.msg import Int8
from std_msgs.msg import Float64MultiArray,Float64
from cv_bridge import CvBridge
import cv2
import os
import cv2.aruco as aruco
import numpy as np
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_1000)
parameters = aruco.DetectorParameters_create()
intr_matrix = np.matrix([[1312.333017, 0.000000, 263.852515],[0, 1300.759011 ,197.130803],[0, 0, 1]])
intr_coeffs = np.matrix([-0.002643, -0.146810, -0.010760, -0.020167, 0.000000])
def img_callback(data):
bridge = CvBridge()
aruco_img = bridge.imgmsg_to_cv2(data, "bgr8")
# aruco_img = cv2.flip(aruco_img,1)
corners, QR_code_id, _ = aruco.detectMarkers(aruco_img,aruco_dict,parameters=parameters)
rvec,tevc = aruco.estimatePoseSingleMarkers(corners, 0.092, intr_matrix, intr_coeffs)
cv2.imshow("frame" , aruco_img)
cv2.waitKey(1)
# 如果检测到二维码
if QR_code_id ==582:
print('detected QR by raw image!.......')
print(QR_code_id)
print('-----------rvec-----------')
print(rvec)
print('-----------tevc-----------')
print(tevc)
print('-----------distance-----------')
offset_x = corners[0][0][0][0]+corners[0][0][1][0]+corners[0][0][2][0]+corners[0][0][3][0]
offset_y = corners[0][0][0][1]+corners[0][0][1][1]+corners[0][0][2][1]+corners[0][0][3][1]
offset = np.array([offset_x/4,offset_y/4],dtype =np.float64)
print(offset)
offset_pub.publish(Float64MultiArray(data=offset))
#amov_gimbal.GimbalAngleControl(roll_rate,pitch_rate,yaw_rate,roll,pitch,yaw)
else:
print("================= not detected ================")
if __name__ == '__main__':
rospy.init_node('qr_code', anonymous=True)
rospy.Subscriber('/image_view/image_raw', Image, img_callback)
offset_pub = rospy.Publisher( 'aruco_offset',Float64MultiArray, queue_size = 10 )
rospy.spin()
amov_gimbal_move.py
订阅位姿话题，进行云台运动控制
Sub：aruco_offset
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
Syntax highlighted code block
import amov_gimbal_sdk_python
import rospy
from move_base_msgs.msg import MoveBaseActionResult
from geometry_msgs.msg import Pose, Point, Quaternion, PoseStamped
from sensor_msgs.msg import Image
from std_msgs.msg import Int8
from std_msgs.msg import Float64MultiArray,Float64
amov_gimbal=amov_gimbal_sdk_python.GimbalInterface()
amov_gimbal.OpenSerial("/dev/ttyUSB0",115200)
amov_gimbal.GetHome()
roll_rate=5
pitch_rate=5
yaw_rate=5
roll=0
pitch=0
yaw=0
def callback(data):
global yaw,pitch
print(data.data[0]-360)
#amov_gimbal.GimbalAngleRateControl(roll,pitch,yaw)
if data.data[0]-360 > 20:
yaw = yaw - (data.data[0]-360)/150
elif data.data[0]-360 < -20:
yaw = yaw - (data.data[0]-360)/150
elif data.data[1]-240 > 20:
pitch = pitch - (data.data[1]-240)/100
elif data.data[1]-240 < -20:
pitch = pitch - (data.data[1]-240)/100
amov_gimbal.GimbalAngleControl(roll_rate,pitch_rate,yaw_rate,roll,pitch,yaw)
#amov_gimbal.CloseSerial()
if __name__ == '__main__':
rospy.init_node('move_node', anonymous=True)
rospy.Subscriber('/aruco_offset',Float64MultiArray, callback)
rospy.spin()
