中国机器人及人工智能大赛
参赛经验：
自主巡航赛道
【机器人和人工智能——自主巡航赛项】动手实践篇-CSDN博客
主要逻辑代码
#!/usr/bin/env python
#coding: utf-8
import rospy
from geometry_msgs.msg import Point
import threading
import actionlib
import time
from actionlib_msgs.msg import GoalStatus
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseWithCovarianceStamped
from tf_conversions import transformations
from ar_track_alvar_msgs.msg import AlvarMarkers
from math import pi
from std_msgs.msg import String
import sys
reload(sys)
sys.setdefaultencoding('utf-8')
class Navigation:
def __init__(self):
self.set_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=5)
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
self.move_base.wait_for_server(rospy.Duration(60))
def set_pose(self, p):
if self.move_base is None:
return False
x, y, th = p
pose = PoseWithCovarianceStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = 'map'
pose.pose.pose.position.x = x
pose.pose.pose.position.y = y
q = transformations.quaternion_from_euler(0.0, 0.0, th/180.0*pi)
pose.pose.pose.orientation.x = q[0]
pose.pose.pose.orientation.y = q[1]
pose.pose.pose.orientation.z = q[2]
pose.pose.pose.orientation.w = q[3]
self.set_pose_pub.publish(pose)
return True
def _feedback_cb(self, feedback):
msg = feedback
#rospy.loginfo("[Navi] navigation feedback\r\n%s"%feedback)
def goto(self, p):
rospy.loginfo("[Navigation] goto %s"%p)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = p[0]
goal.target_pose.pose.position.y = p[1]
q = transformations.quaternion_from_euler(0.0, 0.0, p[2]/180.0*pi)
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
self.move_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb)
result = self.move_base.wait_for_result(rospy.Duration(60))
if not result:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("reach goal %s succeeded!"%p)
return True
def _done_cb(self, status, result):
rospy.loginfo("navigation done! status:%d result:%s"%(status, result))
def _active_cb(self):
rospy.loginfo("[Navi] navigation has be actived")
def cancel(self):
self.move_base.cancel_all_goals()
return True
class ARTracker:
def __init__(self):
self.ar_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.ar_cb)
def ar_cb(self,data):
global tag_id
for marker in data.markers:
tag_id = marker.id
class Object_position:
def __init__(self):
self.ar_sub = rospy.Subscriber('/object_position', Point, self.find_cb)
self.tts_pub = rospy.Publisher('/voiceWords', String, queue_size=10)
def find_cb(self,data):
global find_id
point_msg = data
if(point_msg.z>=1 and point_msg.z<=5):
find_id = 1
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=9 and point_msg.z<=15):
find_id = 2
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=16 and point_msg.z<=23):
find_id = 3
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=25 and point_msg.z<=26):
find_id = 4
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=36 and point_msg.z<=40):
find_id = 5
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=41 and point_msg.z<=43):
find_id = 6
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=70 and point_msg.z<=71):
find_id = 7
self.tts_pub.publish(str(find_id))
elif(point_msg.z>=80 and point_msg.z<=81):
find_id = 8
self.tts_pub.publish(str(find_id))
else:
find_id = 0
#print("id为0，没有识别到！")
def process():
rospy.spin()
find_id = 0
tag_id = 0
both_id =0
if __name__ == '__main__':
rospy.init_node('navigation_demo',anonymous=True)
goalListX = rospy.get_param('~goalListX', '2.0, 2.0,2.0')
goalListY = rospy.get_param('~goalListY', '2.0, 4.0,2.0')
goalListYaw = rospy.get_param('~goalListYaw', '0, 90.0,2.0')
goals = [[float(x), float(y), float(yaw)] for (x, y, yaw) in zip(goalListX.split(","),goalListY.split(","),goalListYaw.split(","))]
##为了方便记忆，goals中[0]是终点，[1]到[8]分别对应场上的8个点，9是第一个框的识别点，10是第二个框的识别点，11是第三个框的识别点，12是第四个框的识别点
object_position = Object_position()
ar_acker = ARTracker()
executor_thread = threading.Thread(target=process).start()
navi = Navigation()
find_point_flag=[0,0,0,0]
have_nav_flag=[0,0,0,0]
time.sleep(10)
navi.goto(goals[9])
find_point_flag[0]=1
while True:
if find_id==1 or tag_id==1:
both_id=1
elif find_id==2 or tag_id==2:
both_id=2
elif find_id==3 or tag_id==3:
both_id=3
elif find_id==4 or tag_id==4:
both_id=4
elif find_id==5 or tag_id==5:
both_id=5
elif find_id==6 or tag_id==6:
both_id=6
elif find_id==7 or tag_id==7:
both_id=7
elif find_id==8 or tag_id==8:
both_id=8
else:
both_id=0
if both_id==0:
if have_nav_flag[0]==1 and find_point_flag[1]==0:
navi.goto(goals[10])
find_point_flag[1]=1
if have_nav_flag[1]==1 and find_point_flag[2]==0:
navi.goto(goals[11])
find_point_flag[2]=1
if have_nav_flag[2]==1 and find_point_flag[3]==0:
navi.goto(goals[12])
find_point_flag[3]=1
if have_nav_flag[3]==1:
navi.goto(goals[0])
break
else:
if both_id==1 and have_nav_flag[0]==0:
navi.goto(goals[1])
have_nav_flag[0]=1
if both_id==2 and have_nav_flag[0]==0:
navi.goto(goals[2])
have_nav_flag[0]=1
if both_id==3 and have_nav_flag[0]==0:
navi.goto(goals[3])
have_nav_flag[1]=1
if both_id==4 and have_nav_flag[0]==0:
navi.goto(goals[4])
have_nav_flag[1]=1
if both_id==5 and have_nav_flag[0]==0:
navi.goto(goals[5])
have_nav_flag[3]=1
if both_id==6 and have_nav_flag[0]==0:
navi.goto(goals[6])
have_nav_flag[3]=1
if both_id==7 and have_nav_flag[0]==0:
navi.goto(goals[7])
have_nav_flag[4]=1
if both_id==8 and have_nav_flag[0]==0:
navi.goto(goals[8])
have_nav_flag[4]=1
#time.sleep(1)
#rclpy.shutdown()
极限4天排队调车，时间太赶了，代码写得很差，一旦识别不了后面的就走不了，后面想重写也没时间。由于官方用的模板匹配识别的太慢，打算用yolov5 -lite模型识别，openvino部署，后面也来不及用上。现在，把yolov5 -lite 大写数字一到八的检测模型 .onnx文件送上，准确率95%
best.zip - 蓝奏云
目标射击赛道
目标射击赛项实践直播回放2024-04-23_哔哩哔哩_bilibili
#!/usr/bin/env python
#coding: utf-8
import rospy
from geometry_msgs.msg import Point, Twist
import threading
import actionlib
import serial
import time
from actionlib_msgs.msg import GoalStatus
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseWithCovarianceStamped
from tf_conversions import transformations
from ar_track_alvar_msgs.msg import AlvarMarkers
from math import pi
import subprocess
class Navigation:
def __init__(self):
self.set_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=5)
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
self.move_base.wait_for_server(rospy.Duration(60))
def set_pose(self, p):
if self.move_base is None:
return False
x, y, th = p
pose = PoseWithCovarianceStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = 'map'
pose.pose.pose.position.x = x
pose.pose.pose.position.y = y
q = transformations.quaternion_from_euler(0.0, 0.0, th/180.0*pi)
pose.pose.pose.orientation.x = q[0]
pose.pose.pose.orientation.y = q[1]
pose.pose.pose.orientation.z = q[2]
pose.pose.pose.orientation.w = q[3]
self.set_pose_pub.publish(pose)
return True
def _feedback_cb(self, feedback):
msg = feedback
#rospy.loginfo("[Navi] navigation feedback\r\n%s"%feedback)
def goto(self, p):
rospy.loginfo("[Navigation] goto %s"%p)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = p[0]
goal.target_pose.pose.position.y = p[1]
q = transformations.quaternion_from_euler(0.0, 0.0, p[2]/180.0*pi)
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
self.move_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb)
result = self.move_base.wait_for_result(rospy.Duration(60))
if not result:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("reach goal %s succeeded!"%p)
return True
def _done_cb(self, status, result):
rospy.loginfo("navigation done! status:%d result:%s"%(status, result))
def _active_cb(self):
rospy.loginfo("[Navi] navigation has be actived")
def cancel(self):
self.move_base.cancel_all_goals()
return True
class ARTracker:
def __init__(self):
self.ar_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.ar_cb)
def ar_cb(self,data):
global target_id
global ar_x
global ar_y
global ar_z
global ar_id
for marker in data.markers:
if marker.id == target_id :
ar_x = marker.pose.pose.position.x
ar_y = marker.pose.pose.position.y
ar_z = marker.pose.pose.position.z
ar_id = marker.id
#print('AR Marker Position (x,y,z):', ar_x, ar_y,ar_z)
break
class Object_position:
def __init__(self):
self.ar_sub = rospy.Subscriber('/object_position', Point, self.find_cb)
def find_cb(self,data):
global find_id
global find_x
global find_y
point_msg = data
if(point_msg.z>=1 and point_msg.z<=5):
find_id = 1
find_x=point_msg.x
find_y=point_msg.y
else:
find_id = 0
def process():
rospy.spin()
find_id = 0
find_x=0.0
find_y=0.0
target_id = 0
ar_id = 0
ar_x =0.0
ar_y =0.0
ar_z =0.0
if __name__ == '__main__':
rospy.init_node('navigation_demo',anonymous=True)
ser = serial.Serial(port="/dev/shoot", baudrate=9600, parity="N", bytesize=8, stopbits=1)
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1000)
goals = [ [1.1 , -0.37,0.0],
[1.1 , -1.45,.0],
[1.0 , -2.72,.0],
[0.07 , -2.72,.0] ]
object_position = Object_position()
ar_acker = ARTracker()
executor_thread = threading.Thread(target=process).start()
navi = Navigation()
time.sleep(15)
# ------first--------------------------------------------------------
navi.goto(goals[0])
start=time.time()
is_shoot=0
while True:
if find_id == 1:
flog0 = find_x - 320
flog1 = abs(flog0)
print(flog0)
if abs(flog1) >10:
msg = Twist()
msg.angular.z = -0.01 * flog0
pub.publish(msg)
print(msg.angular.z)
elif abs(flog1) <= 10:
print('1 is shoot')
ser.write(b'\x55\x01\x12\x00\x00\x00\x01\x69')
time.sleep(0.2)
ser.write(b'\x55\x01\x11\x00\x00\x00\x01\x68')
time.sleep(2)
is_shoot=1
break
end=time.time()
if end-start>12:
break
if is_shoot==0:
ser.write(b'\x55\x01\x12\x00\x00\x00\x01\x69')
time.sleep(0.2)
ser.write(b'\x55\x01\x11\x00\x00\x00\x01\x68')
time.sleep(2)
#subprocess.run( ['rosnode','kill','find_object_2d'] )
# ------sencond-----------------------------------------------------
navi.goto(goals[1])
target_id=1
Yaw_th = 0.003
start=time.time()
is_shoot=0
while True:
if ar_id == target_id:
ar_x_abs = abs(ar_x)
print('x:',ar_x)
print('z:',ar_z)
if ar_x_abs >= Yaw_th:
msg = Twist()
msg.angular.z = -1.5 * ar_x
pub.publish(msg)
elif ar_x_abs < Yaw_th and (ar_z >= 1.57 and ar_z <=1.64):
print('2 is shoot')
ser.write(b'\x55\x01\x12\x00\x00\x00\x01\x69')
time.sleep(0.1)
ser.write(b'\x55\x01\x11\x00\x00\x00\x01\x68')
is_shoot=1
break
end=time.time()
if end-start>20:
break
if is_shoot==0:
ser.write(b'\x55\x01\x12\x00\x00\x00\x01\x69')
time.sleep(0.2)
ser.write(b'\x55\x01\x11\x00\x00\x00\x01\x68')
time.sleep(2)
# # --------------------third----------------------------------
navi.goto(goals[2])
target_id=2 # ------------------------------------------------------
Yaw_th = 0.002
start=time.time()
is_shoot=0
while True:
if ar_id == target_id:
ar_x_abs = abs(ar_x)
print(ar_x)
if ar_x_abs >= Yaw_th:
msg = Twist()
msg.angular.z = -1.5 * ar_x
pub.publish(msg)
elif ar_x_abs < Yaw_th:
print('3 is shoot')
ser.write(b'\x55\x01\x12\x00\x00\x00\x01\x69')
time.sleep(0.1)
ser.write(b'\x55\x01\x11\x00\x00\x00\x01\x68')
is_shoot=1
break
end=time.time()
if end-start>12:
break
if is_shoot==0:
ser.write(b'\x55\x01\x12\x00\x00\x00\x01\x69')
time.sleep(0.2)
ser.write(b'\x55\x01\x11\x00\x00\x00\x01\x68')
time.sleep(2)
# # -------------------------------------------------------------------------
navi.goto(goals[3])
#slowly
#rclpy.shutdown()
调的时候旋转靶是可以打的，但是比赛过程就旋转靶没打中，没办法了，就这样。
经验
ssh -Y abot@192.168.135.6
sudo nautilus
scp -r abot@192.168.135.6:/home/abot/robot_ws D:主目录运行：
建图： ./1-gmapping.sh 保存： roslaunch robot_slam save_map.launch
射击： roslaunch abot_bringup shoot.launch 发射驱动程序
rostopic pub /shoot std_msgs/String "data: '' " 发布射击的空话题，等待发射
识别：
roslaunch usb_cam usb_cam_test.launch 打开相机
rosrun rqt_image_view rqt_image_view 可视化相机
语音：
连接蓝牙耳机WI-C200
roscore
rosrun robot_voice tts_subscribe
rostopic pub /voiceWords std_msgs/String "data: '1234' "
启动导航与识别：
3-mission.sh 在这里
roslaunch track_tag usb_cam_with_calibration.launch 打开相机节点
roslaunch track_tag ar_track_camera.launch 启动二维码识别节点
rosrun robot_voice tts_subscribe; exec bash 语音播报节点
robot_slam/launch/multi_goal.launch 修改导航的目标点的坐标值
robot_slam/scripts/ navigation_multi_goals.py 修改对应id分别走到哪个点
--------------------------------------------------------------------------------------把官方给的代码放到 src\robot_slam\scripts 里面
--------------------1. 自主巡航------------------------------
修改导航的目标点的位姿： robot_slam/launch/multi_goal.launch
X Y Yaw 一列为一组值 ，一一对应（分别表示goal[0] ...goal[1]）， Yaw是角度制，90.0表示逆时针旋转90度（正值向左）识别：（视频的第35分钟）
roslaunch usb_cam usb_cam_test.launch 打开相机
roslaunch find_object_2d find_object_2d6.launch 启动识别程序Edit -- Add object -- Take picture截图 --框选物体 --End --File --Save object 路径选择 abot_find/object/
然后对图片进行特殊命名（数字范围）
-------没有ar_cb函数py
--------------------2. 目标射击------------------------------user_demo/param/mission.yaml 修改射击目标点的相关参数
roslaunch usb_cam usb_cam_test.launch 打开相机
roslaunch find_object_2d find_object_2d6.launch 启动识别程序
rosrun robot_slam III.py 识别结果判断
rostopic echo /object_position跟踪标签：在6-mission.sh里有 ，
roslaunch track_tag usb_cam_with_calibration.launch
roslaunch track_tag ar_track_camera.launchrostopic echo /ar_pose_marker
然后运行官方给的代码 rosrun robot_slam .py ，需要把代码整合起来，能够识别三种目标并射击
启动代码前一定要插上炮台的USB串口线，不然运行就会报错没有串口 /dev/shoot
记得打开炮台开关
---------------------------------------------------------------------------
查看坐标点
运行navigation.sh脚本前注释掉最后一行 ，在打开的地图里点击目标点前 运 rostopic echo /move_base_simple/goal
在导航地图中使用RViz中的navigation goal标定目标后，到终端的输出查看pose 字段，里面有x,y目标点
直接拿迟来量坐标比较快，单位是米，搞懂ros坐标系编译及运行--------------------------------------------------------------------
catkin_make
catkin_make -DCATKIN_WHITELIST_PACKAGES="robot_slam"source devel/setup.bash
source /opt/ros/melodic/setup.bash
参加比赛要注重交流，从比赛中获得友情，知识和实践，面对困难永不放弃的决心，不要把奖项看的那么重要。
在小车的主机里插入鼠标，连接WIFI，使用屏幕键盘输入密码，剩下的交给远程控制。
同一局域网下ssh传输文件：
无论是windows还是ubuntu，都可以互传文件
同一局域网多人连接：
一个主要的人负责用向日葵远程控制小车，进行调试
一个次要的人负责使用ssh连接，在里面写代码，查看文档
等调完半小时，换另一个远程控制调车。
（Vscode的ssh远程连接修改代码）（或者，在虚拟机里ssh连接小车主机<加Y参数 ssh -Y IP>，在终端里执行 sudo nautilus 打开并编辑文件管理器）
功能包重名，就修改launch文件名区别开，如过.sh脚本运行报错，单独分开命令运行试试，记得source自己的工作空间
复制别人的工作空间，需要重新编译，不然setup.bash还是用的原来的，串到原来的代码
编译：
catkin_make # 若失败，删除build目录和devel目录试试
catkin_make -DCATKIN_WHITELIST_PACKAGES="robot_slam"
catkin build 包名
Ctrl+h 修改环境变量.bashrc
......
