自主泊车算法

#!/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[2Jx1b[H‘）
        rospy.loginfo（‘x1b[1M
‘
                      ‘33[34;1mSpeed: 33[32;1m%0.2f m/s ‘
                      ‘33[34;1mSteering Angle: 33[32;1m%0.1f rad33[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-masterREADME.md
     目录           0  2017-12-06 15:57  Automatic-Parking-masterackermann_model
     目录           0  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleop
     文件         515  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleopCMakeLists.txt
     文件        7652  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleopLICENSE
     文件        1045  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleopREADME.md
     文件         140  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleopuild.sh
     文件         587  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleopcontrol
     文件         656  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleopdebpkg.sh
     目录           0  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleoplaunch
     文件         446  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleoplaunchackermann_drive_joyop.launch
     文件         451  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleoplaunchackermann_drive_keyop.launch
     文件         454  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleoppackage.xml
     目录           0  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleopscripts
     文件        2192  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleopscriptsackermann_drive_joyop.py
     文件        4570  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_drive_teleopscriptsackermann_drive_keyop.py
     目录           0  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_msgs-master
     文件          63  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_msgs-master.gitignore
     文件         624  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_msgs-masterCHANGELOG.rst
     文件         431  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_msgs-masterCMakeLists.txt
     文件         366  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_msgs-masterREADME.rst
     文件         163  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_msgs-mastermainpage.dox
     目录           0  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_msgs-mastermsg
     文件        1810  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_msgs-mastermsgAckermannDrive.msg
     文件         120  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_msgs-mastermsgAckermannDriveStamped.msg
     文件         794  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_msgs-masterpackage.xml
     目录           0  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_vehicle_description
     文件         124  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_vehicle_descriptionCMakeLists.txt
     文件         247  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_vehicle_descriptioncmd_line
     目录           0  2017-12-06 15:57  Automatic-Parking-masterackermann_modelackermann_vehicle_descriptionlaunch
............此处省略315个文件信息