中国机器人及人工智能大赛
参赛经验:
自主巡航赛道
【机器人和人工智能——自主巡航赛项】动手实践篇-CSDN博客
主要逻辑代码
#!/usr/bin/env python
#coding: utf-8import 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 Falsex, y, th = ppose = PoseWithCovarianceStamped()pose.header.stamp = rospy.Time.now()pose.header.frame_id = 'map'pose.pose.pose.position.x = xpose.pose.pose.position.y = yq = 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 Truedef _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 Trueclass ARTracker:def __init__(self):self.ar_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.ar_cb)def ar_cb(self,data):global tag_idfor marker in data.markers:tag_id = marker.idclass 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 = dataif(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 = 2self.tts_pub.publish(str(find_id))elif(point_msg.z>=16 and point_msg.z<=23):find_id = 3self.tts_pub.publish(str(find_id))elif(point_msg.z>=25 and point_msg.z<=26):find_id = 4self.tts_pub.publish(str(find_id))elif(point_msg.z>=36 and point_msg.z<=40):find_id = 5self.tts_pub.publish(str(find_id))elif(point_msg.z>=41 and point_msg.z<=43):find_id = 6self.tts_pub.publish(str(find_id))elif(point_msg.z>=70 and point_msg.z<=71):find_id = 7self.tts_pub.publish(str(find_id))elif(point_msg.z>=80 and point_msg.z<=81):find_id = 8self.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 =0if __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]=1while True:if find_id==1 or tag_id==1:both_id=1elif find_id==2 or tag_id==2:both_id=2elif find_id==3 or tag_id==3:both_id=3elif find_id==4 or tag_id==4:both_id=4elif find_id==5 or tag_id==5:both_id=5elif find_id==6 or tag_id==6:both_id=6elif find_id==7 or tag_id==7:both_id=7elif find_id==8 or tag_id==8:both_id=8else: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]=1if have_nav_flag[1]==1 and find_point_flag[2]==0: navi.goto(goals[11]) find_point_flag[2]=1if have_nav_flag[2]==1 and find_point_flag[3]==0:navi.goto(goals[12])find_point_flag[3]=1if have_nav_flag[3]==1: navi.goto(goals[0]) breakelse:if both_id==1 and have_nav_flag[0]==0: navi.goto(goals[1])have_nav_flag[0]=1if both_id==2 and have_nav_flag[0]==0: navi.goto(goals[2])have_nav_flag[0]=1if both_id==3 and have_nav_flag[0]==0: navi.goto(goals[3])have_nav_flag[1]=1if both_id==4 and have_nav_flag[0]==0: navi.goto(goals[4])have_nav_flag[1]=1if both_id==5 and have_nav_flag[0]==0: navi.goto(goals[5])have_nav_flag[3]=1if both_id==6 and have_nav_flag[0]==0: navi.goto(goals[6])have_nav_flag[3]=1if both_id==7 and have_nav_flag[0]==0: navi.goto(goals[7])have_nav_flag[4]=1if 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-8import 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 subprocessclass 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 Falsex, y, th = ppose = PoseWithCovarianceStamped()pose.header.stamp = rospy.Time.now()pose.header.frame_id = 'map'pose.pose.pose.position.x = xpose.pose.pose.position.y = yq = 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 Truedef _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 Trueclass ARTracker:def __init__(self):self.ar_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.ar_cb)def ar_cb(self,data):global target_idglobal ar_xglobal ar_yglobal ar_zglobal ar_idfor marker in data.markers:if marker.id == target_id :ar_x = marker.pose.pose.position.xar_y = marker.pose.pose.position.yar_z = marker.pose.pose.position.zar_id = marker.id#print('AR Marker Position (x,y,z):', ar_x, ar_y,ar_z)breakclass 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_xglobal find_y point_msg = dataif(point_msg.z>=1 and point_msg.z<=5):find_id = 1 find_x=point_msg.xfind_y=point_msg.y else:find_id = 0def 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.0if __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=0while True:if find_id == 1:flog0 = find_x - 320flog1 = 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=1breakend=time.time()if end-start>12:breakif 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.003start=time.time()is_shoot=0while 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=1breakend=time.time()if end-start>20:breakif 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.002start=time.time()is_shoot=0while 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 breakend=time.time()if end-start>12:breakif 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
......