def __init__(self):
# define vehicle parameters
self.gear = 1 # fixed to gear 1
self.VELOCITY_FORWARD_MAXIMUM = 10. # in m/s
self.VELOCITY_REVERSE_MAXIMUM = 3.
self.STEERING_MAXIMUM = np.pi / 4. # rad
# define output variables (si)
self.output_steering = 0.
self.output_velocity = 0.
# define output variables (%)
self.output_steering_ppm = 0
self.output_velocity_ppm = 0
# define user input variables (%)
self.key = ""
self.user_input_steering_ppm = 0
self.user_input_velocity_ppm = 0
# define parametrs for user interface
self.CONTROL_OUTPUT_SATURATION = 100 # set training mode here (e.g. only 20 % control output)
self.STEP_VELOCITY = 5
self.STEP_STEERING = 20
self.USER_INPUT = False # boolean flag for key entry by user
self.MANUAL_CONTROL_WATCHDOG_IS_GOOD = False # if this is false something in the class went wrong
# Initialise ROS
self.PUBLISH_ACTIVE = False
self.ROS_PUBLISH_RATE = 40
self.ros_timer = 0.0
self.ros_msg_out = msg_out()
# ros related timing constraints on reading input (UNIX only)
self.current_time = time.time()
self.minimum_time_period = 1. / self.ROS_PUBLISH_RATE
sg.signal(sg.SIGALRM, self.ros_publish_sig_handle)
sg.setitimer(sg.ITIMER_REAL, 0, self.minimum_time_period)
# define node and publisher objects
rp.init_node('manual_control', anonymous=True)
self.ros_publisher = rp.Publisher('/lli/ctrl_request',
msg_out,
queue_size=0)
self.ros_publish_rate = rp.Rate(self.ROS_PUBLISH_RATE)
# define update and status flags
self.CONTROL_REQUEST_SUBMITTED = False # set to true on each publish event
#!/usr/bin/env python
import rospy
from low_level_interface.msg import lli_ctrl_request as msg_out
from tf.transformations import euler_from_quaternion
from numpy import *
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
from Parallel_parking import FollowThenPark
ros_out = msg_out()
ros_out.velocity = 14
vel = 0
steer = 0
NPOINTS = 100
DETECTED = False
LOOP = True
WAITING_FOR_START = False
#current_point
path = []
yr = 0
xr = 0
xo = 0
yo = 0
zo = 0
w = 0
pub = rospy.Publisher('lli/ctrl_request', msg_out, queue_size=10)