文章目录
任务信息
自动巡航功能
roslaunch启动
roslaunch 功能包 指定的launch文件
rosrun启动
rosrun 功能包 指定的.py / .cpp文件
手把手实现代码
初始代码
#!/usr/bin/env python
#coding: utf-8
import rospy
import math
import actionlib
import serial
import time
from std_msgs.msg import String
from actionlib_msgs.msg import *
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseWithCovarianceStamped
from tf_conversions import transformations
from math import pi
from std_msgs.msg import String
import sys
reload(sys)
sys.setdefaultencoding('utf-8')
import os
class navigation_demo:
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 _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 _feedback_cb(self, feedback):
msg = feedback
#rospy.loginfo("[Navi] navigation feedback\r\n%s"%feedback)
def goto(self, p):
rospy.loginfo("[Navi] 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 cancel(self):
self.move_base.cancel_all_goals()
return True
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(","))]
print ('Please 1 to continue: ')
input = raw_input()
r = rospy.Rate(1)
r.sleep()
navi = navigation_demo()
navi.goto(goals[0])
rospy.sleep(5)
navi.goto(goals[2])
rospy.sleep(5)
while not rospy.is_shutdown():
r.sleep()
添加二维码识别
修改防报错
导入库
添加全局变量id，订阅者，ar_cb函数
添加图像数字识别
添加库文件
添加全局变量find_id，订阅者，find_id识别函数
添加语音播报
指定音乐路径，要根据自己情况修改
执行指令后进行语音播报
添加小车行驶路径
添加区域一的if语句，其余区域也一样。
手动对图片分类
讲解视频
其余篇章
1.【机器人和人工智能——自主巡航赛项】基础篇
2.【机器人和人工智能——自主巡航赛项】进阶篇