rospy控制小车多点巡航

rospy控制小车多点巡航

rospy控制小车多点巡航

场景：如何实现小车按照规定的路线自主的巡航
这里我采用小车多点导航来实现，且加入附近约束与超时共同作为条件约束。资源服务当出现优先级高于巡航的任务时，中断小车当前的巡航，从而执行新的/move_base任务。

#!/usr/bin/env python
#coding=utf-8
import rospy
import tf
import time
import PyKDL
import actionlib
from nav_msgs.msg import Path
from actionlib_msgs.msg import *
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseWithCovarianceStamped
from tf_conversions import transformations
from math import radians, copysign, sqrt, pow, pi, atan2, cos, sin
from mux.srv import ControlStateclass Navigation_multi:def __init__(self,robot_name="robot1", wait_time=1800, nearest_r=2):bot_name = ve_base = actionlib.SimpleActionClient(bot_name)+"/move_base", ve_base.wait_for_server(rospy.Duration(60))arest_r=nearest_rself.wait_time = _shutdown(self.shutdown)# 四位数转欧拉角def quat_to_angle(self,quat):rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)return rot.GetRPY()[2]# 当前位置def get_amcl_pose(self):msg = rospy.wait_for_message(bot_name)+"/amcl_pose",PoseWithCovarianceStamped,timeout=None)return msg.pose.pose.position, self.quat_to_angle(msg.ientation)# 当前位置到目标位置之间距离def stop_pose_distance(self,pose):(position, rotation) = _amcl_pose()return sqrt(pow((position.x - pose[0]), 2) + pow((position.y - pose[1]), 2))# 打印启动位置与目标位置def print_point(self, pose):position = _amcl_pose()[0]rospy.loginfo("Start pose: x:%s y:%s    ---------->    Stop pose: x:%s y:%s", position.x, position.y, pose[0], pose[1])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):rospy.loginfo("[Navi] navigation feedbackrn%s"%feedback)# 资源的请求与释放 请求返回True Falsedef control_client(self, permission, state):rospy.wait_for_service(bot_name)+"/control_server")try:client = rospy.ServiceProxy(bot_name)+"/control_server", ControlState)response = client(permission,state)lexcept:return Falsedef shutdown(self):# Always stop the robot when shutting down the l_client(1,0)rospy.loginfo("Stopping ")ve_base.cancel_goal()rospy.sleep(1)# 多目标附近巡航def go_on_patrol(self, goals):for pose in goals:goal = MoveBaseGoal()goal.target_pose.header.frame_id = 'map'goal.target_pose.header.stamp = w()goal.target_pose.pose.position.x = pose[0]goal.target_pose.pose.position.y = pose[1]q = transformations.quaternion_from_euler(0.0, 0.0, pose[2]/180.0*pi)goal.target_ientation.x = q[0]goal.target_ientation.y = q[1]goal.target_ientation.z = q[2]goal.target_ientation.w = q[3]# 向控制发送 优先级：1 | 请求资源：1(1请求资源，0释放资源)l_client(1,1):# 休眠两秒等待资源释放rospy.sleep(2)# 发送打印启动--->目标self.print_point(ve_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb)# 记录启动时间start = time.time()# 设定检测频率：检测是否到达目标附近频率、检测是否超时频率、检测是否资源被优先级高抢占频率rate = rospy.Rate(1)while True:if rospy.is_shutdown():rospy.loginfo("Ros is shutdown!")breakelif float(self.stop_pose_distance(pose)) < arest_r):rospy.loginfo("The robot reaches the nearest constraint")breakelif int(time.time()-start) > int(self.wait_time):rospy.loginfo("Time out")l_client(1,1) == False:ros.loginfo("Resource occupied")breakrate.sleep()# 取消目标请求ve_base.cancel_goal()# 释放资源l_client(1,0)rospy.sleep(2)else:rospy.loginfo("Resource request failed")rospy.loginfo("Current location n <------------> n %s",pose)breakif __name__ == "__main__":rospy.init_node('Navigation_multi',anonymous=True)robot_id = _param('~robot_num','3')robot_name = 'robot'+ str(robot_id)goalListX = _param('~goalListX', '2.0, 2.0')goalListY = _param('~goalListY', '2.0, 4.0')goalListYaw = _param('~goalListYaw', '0, 90.0')goals = [[float(x), float(y), float(yaw)] for (x, y, yaw) in zip(goalListX.split(","),goalListY.split(","),goalListYaw.split(","))]navi = Navigation_multi(robot_name=robot_name,wait_time=1800, nearest_r=_on_patrol(goals)rospy.sleep(1)

总结

通过简单优先级服务，防止资源抢占问题