自主泊车算法

可以 实现自主泊车

#!/usr/bin/env python

‘‘‘
ackermann_drive_joyop.py:
    A ros joystick teleoperation script for ackermann steering based robots
‘‘‘

__author__ = ‘George Kouros‘
__license__ = ‘GPLv3‘
__maintainer__ = ‘George Kouros‘
__email__ = ‘gkourosg@yahoo.gr‘

import rospy
from ackermann_msgs.msg import AckermannDrive
from sensor_msgs.msg import Joy
import sys

class AckermannDriveJoyop:

    def __init__（self args）:
        if len（args） == 2:
            self.max_speed = float（args[0]）
            self.max_steering_angle = float（args[1]）
        else:
            self.max_speed = 0.2
            self.max_steering_angle = 0.7

        self.speed = 0
        self.steering_angle = 0
        self.joy_sub = rospy.Subscriber（‘/joy‘ Joy self.joy_callback）
        self.drive_pub = rospy.Publisher（‘ackermann_cmd‘ AckermannDrive
                                         queue_size=1）
        rospy.Timer（rospy.Duration（1.0/5.0） self.pub_callback oneshot=False）
        rospy.loginfo（‘ackermann_drive_joyop_node initialized‘）

    def joy_callback（self joy_msg）:
        self.speed = joy_msg.axes[2] * self.max_speed;
        self.steering_angle = joy_msg.axes[3] * self.max_steering_angle;


    def pub_callback（self event）:
        ackermann_cmd_msg = AckermannDrive（）
        ackermann_cmd_msg.speed = self.speed
        ackermann_cmd_msg.steering_angle = self.steering_angle
        self.drive_pub.publish（ackermann_cmd_msg）
        self.print_state（）

    def print_state（self）:
        sys.stderr.write（‘\x1b[2J\x1b[H‘）
        rospy.loginfo（‘\x1b[1M\r‘
                      ‘\033[34;1mSpeed: \033[32;1m%0.2f m/s ‘
                      ‘\033[34;1mSteering Angle: \033[32;1m%0.1f rad\033[0m‘
                      self.speed self.steering_angle）

    def finalize（self）:
        rospy.loginfo（‘Halting motors aligning wheels and exiting...‘）
        ackermann_cmd_msg = AckermannDrive（）
        ackermann_cmd_msg.speed = 0
        ackermann_cmd_msg.steering_angle = 0
        self.drive_pub.publish（ackermann_cmd_msg）
        sys.exit（）

if __name__ == ‘__main__‘:
    rospy.init_node（‘ackermann_drive_joyop_node‘）
    joyop = AckermannDriveJoyop（sys.argv[1:3]）
    rospy.spin（）

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-12-06 15:57  Automatic-Parking-master\
     文件        1462  2017-12-06 15:57  Automatic-Parking-master\README.md
     目录           0  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\
     目录           0  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\
     文件         515  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\CMakeLists.txt
     文件        7652  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\LICENSE
     文件        1045  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\README.md
     文件         140  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\build.sh
     文件         587  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\control
     文件         656  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\debpkg.sh
     目录           0  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\launch\
     文件         446  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\launch\ackermann_drive_joyop.launch
     文件         451  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\launch\ackermann_drive_keyop.launch
     文件         454  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\package.xml
     目录           0  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\scripts\
     文件        2192  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\scripts\ackermann_drive_joyop.py
     文件        4570  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_drive_teleop\scripts\ackermann_drive_keyop.py
     目录           0  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_msgs-master\
     文件          63  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_msgs-master\.gitignore
     文件         624  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_msgs-master\CHANGELOG.rst
     文件         431  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_msgs-master\CMakeLists.txt
     文件         366  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_msgs-master\README.rst
     文件         163  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_msgs-master\mainpage.dox
     目录           0  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_msgs-master\msg\
     文件        1810  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_msgs-master\msg\AckermannDrive.msg
     文件         120  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_msgs-master\msg\AckermannDriveStamped.msg
     文件         794  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_msgs-master\package.xml
     目录           0  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_vehicle_description\
     文件         124  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_vehicle_description\CMakeLists.txt
     文件         247  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_vehicle_description\cmd_line
     目录           0  2017-12-06 15:57  Automatic-Parking-master\ackermann_model\ackermann_vehicle_description\launch\
............此处省略315个文件信息