rospy 控制小车定点旋转侦察

rospy 控制小车定点旋转侦察

场景：控制小车到达一个指定位置附近后，开始旋转侦察

#!/usr/bin/env python
#coding=utf-8
import PyKDL
import rospy
import actionlib
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,Twist, Point, Quaternion
import tf
from tf_conversions import transformations
from math import radians, copysign, sqrt, pow, pi
import time
from mux.srv import ControlState


class Navigation_around:
    def __init__(self, robot_name="robot1", nearest_r=2, wait_time=600):
        self.robot_name = robot_name
        self.nearest_r = nearest_r
        self.wait_time = wait_time
        self.move_base = actionlib.SimpleActionClient(str(self.robot_name)+"/move_base", MoveBaseAction)
        self.move_base.wait_for_server(rospy.Duration(60))
        rospy.on_shutdown(self.shutdown)
    
    # robot当前位置
    def get_amcl_pose(self):
        msg = rospy.wait_for_message(str(self.robot_name)+"/amcl_pose",PoseWithCovarianceStamped,timeout=None)
        return msg.pose.pose.position, self.quat_to_angle(msg.pose.pose.orientation)

    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 feedback\r\n%s"%feedback)

    # 当前位置与目标位置距离  
    def stop_pose_distance(self,pose):
        (position, rotation) = self.get_amcl_pose()
        return sqrt(pow((position.x - pose[0]), 2) + pow((position.y - pose[1]), 2))
    
    # 四位数转欧拉角
    def quat_to_angle(self,quat):
        rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
        return rot.GetRPY()[2]
    # 退出关闭
    def shutdown(self):
        rospy.loginfo("Stopping the robot....")
        self.control_client(3,0)
        self.move_base.cancel_goal()
        rospy.sleep(1)

    # 资源的请求与释放 请求返回True False
    def control_client(self, permission, state):
        rospy.wait_for_service(str(self.robot_name)+"/control_server")
        try:
            control_client = rospy.ServiceProxy(str(self.robot_name)+"/control_server", ControlState)
            response = control_client(permission, state)
            return response.control
        except:
            return False

     # 打印启动位置与目标位置
    def print_point(self, pose):
        position = self.get_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 get_to(self, pose):
        rospy.loginfo("[Navi] goto %s",pose)
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = 'map'
        goal.target_pose.header.stamp = rospy.Time.now()
        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_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]

        # 向控制发送 优先级：3 | 请求资源：1(1请求资源，0释放资源)
        if self.control_client(3,1):
            # 休眠两秒等待资源释放
            rospy.sleep(2)

            # 发送打印启动--->目标
            self.print_point(pose)
            self.move_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!")
                    # 取消目标请求
                    self.move_base.cancel_goal()
                    # 释放资源
                    self.control_client(3,0)
                    return False
                elif float(self.stop_pose_distance(pose)) < float(self.nearest_r):
                    rospy.loginfo("The robot reaches the nearest constraint")
                    # 取消目标请求
                    self.move_base.cancel_goal()
                    # 释放资源
                    self.control_client(3,0)
                    return True

                elif int(time.time()-start) > int(self.wait_time):
                    rospy.loginfo("Time out")
                    # 取消目标请求
                    self.move_base.cancel_goal()
                    # 释放资源
                    self.control_client(3,0)

                    return False

                elif self.control_client(3,1) == False:
                    ros.loginfo("Resource occupied")
                    # 取消目标请求
                    self.move_base.cancel_goal()
                    # 释放资源
                    self.control_client(3,0)

                    return False
                rate.sleep()
        else:
            rospy.loginfo("Resource request failed")
            rospy.loginfo("Current location \n <------------> \n %s",pose)
            return False
            
    
class OutAndBack():
    def __init__(self,robot_name="robot1", rotation_angle=360, pause_angle=30, angular_speed=0.2, slip_offset=5, pause_time=2):
        '''
        robot_name: 
        rotation_angle: 旋转角度默认360
        pause_angle: 停顿角度默认30
        angular_speed :默认角速度0.3
        pause_time: 停顿时间默认2
        '''
        self.robot_name = robot_name
        self.rotation_angle = radians(rotation_angle)
        self.pause_angle = radians(pause_angle)
        self.angular_speed = angular_speed
        self.slip_offset = radians(slip_offset)
        self.pause_time = pause_time
        
        # 移动指令订阅
        self.mobile_base_velocity = rospy.Publisher(str(self.robot_name)+'/mobile_base/commands/velocity', Twist, queue_size=1)
        # 关闭时执行
        rospy.on_shutdown(self.shutdown)
        rate = rospy.Rate(10)
        rospy.sleep(2)
        

        self.empty_cmd()
        rospy.sleep(1)
        for i in range(int(self.rotation_angle/self.pause_angle)):

            # 旋转操作设置 角速度
            move_cmd = Twist()
            move_cmd.angular.z = self.angular_speed

            # 记录当前位置
            (position, rotation) = self.get_amcl_pose()
            # 检查旋转是否到达停顿角 | 是否关闭
            while True:
                if abs(self.normalize_angle(rotation - self.get_amcl_pose()[1]))+self.slip_offset >= abs(self.pause_angle):
                    rospy.loginfo("Around degrees %s",self.pause_angle*(i+1)*180/3.14)
                    self.empty_cmd()
                    break
                elif rospy.is_shutdown():
                    rospy.loginfo("Ros is shutdown !")
                    break
                self.mobile_base_velocity.publish(move_cmd)
                rate.sleep()
            
            # 清空cmd 停顿等待
            self.empty_cmd()
            rospy.sleep(self.pause_time)
        
    
    # 清空指令控制 | 感觉休息会好点
    def empty_cmd(self):
        rospy.sleep(0.5)
        self.mobile_base_velocity.publish(Twist())
        rospy.sleep(0.5)

    # 获取当前位置
    def get_amcl_pose(self):
        msg = rospy.wait_for_message(str(self.robot_name)+"/amcl_pose",PoseWithCovarianceStamped,timeout=None)
        return msg.pose.pose.position, self.quat_to_angle(msg.pose.pose.orientation)
    
    # 关闭时处理
    def shutdown(self):
        # Always stop the robot when shutting down the node.
        rospy.loginfo("Stopping the robot...")
        self.mobile_base_velocity.publish(Twist())
        rospy.sleep(1)

    # 四位数转欧拉角
    def quat_to_angle(self,quat):
        rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
        return rot.GetRPY()[2]
    
    # 表示角度转换[-pi,pi]
    def normalize_angle(self,angle):
        res = angle
        while res > pi:
            res -= 2.0*pi
        while res < -pi:
            res += 2.0*pi
        return res

if __name__ == "__main__":

    robot_id = rospy.get_param('~robot_num','3')
    robot_name = 'robot'+ str(robot_id)
    rospy.init_node('Navigation_around',anonymous=True)
    pose = [3.0,3.0,90.0]

    try:
        navi = Navigation_around(robot_name=robot_name, nearest_r=2, wait_time=600)
        if navi.get_to(pose):
            OutAndBack(robot_name=robot_name, rotation_angle=360, pause_angle=60, angular_speed=0.2, pause_time=2)
            rospy.loginfo("OK")
        else:
            rospy.loginfo("Get to fail")
    except:
        rospy.loginfo("Get to fail.")

注意

在控制旋转时，旋转速度过快根据硬件情况会出现一定偏差