自主巡航，目标射击

中国机器人及人工智能大赛

参赛经验：

自主巡航赛道

【机器人和人工智能——自主巡航赛项】动手实践篇-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.launch

rostopic 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

......