自主巡航,目标射击 中国机器人及人工智能大赛

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

参赛经验:

自主巡航赛道

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

...... 

相关推荐

最近更新

  1. docker php8.1+nginx base 镜像 dockerfile 配置

    2024-07-20 23:50:03       142 阅读
  2. Could not load dynamic library ‘cudart64_100.dll‘

    2024-07-20 23:50:03       156 阅读
  3. 在Django里面运行非项目文件

    2024-07-20 23:50:03       131 阅读
  4. Python语言-面向对象

    2024-07-20 23:50:03       141 阅读

热门阅读

  1. 【面试题】Golang 锁的相关问题(第七篇)

    2024-07-20 23:50:03       24 阅读
  2. Perl编程艺术:探索代码重用的无限可能

    2024-07-20 23:50:03       28 阅读
  3. Python 基础——列表(list)

    2024-07-20 23:50:03       30 阅读
  4. jvm-证明cpu指令是乱序执行的案例

    2024-07-20 23:50:03       30 阅读
  5. django 应用目录介绍

    2024-07-20 23:50:03       34 阅读