def set_steer(data):
car_ctl = AckermannDrive()
car_msg = AckermannDriveStamped()
steering_angle = rospy.Publisher('/vesc/racecar/ackermann_cmd_mux/input/teleop', AckermannDriveStamped, queue_size=1)
k_distance = 6.0
k_angle = 2.0
if data.data[0] and data.data[3]:
print "section 1"
#error = k_distance * (data.data[4] - data.data[1])
error = k_angle * (90.0 - data.data[2])
print "k_distance", k_distance, "data.data[4]", data.data[4], "data_data[1]", data.data[1], "error", error
elif data.data[0] and not data.data[3]:
print "section 2"
error = k_angle * (90.0 - data.data[2])
elif data.data[3] and not data.data[0]:
print "section 3"
error = k_angle * (-90.0 - data.data[5])
else:
print "section 4"
pass
#car_ctl.acceleration = 0.0
car_ctl.speed = 0.8
car_ctl.steering_angle = error
#steering_angle.publish('{drive: {steering_angle: ' + str(error) + ', speed: ' + str(prev_speed) + '}}')
#rospy.Publisher('ackermann_cmd', AckermannDriveStamped)
"publish pilot command message"
car_msg.header.stamp = rospy.Time.now()
car_msg.header.frame_id = "wall_follower"
car_msg.drive = car_ctl
print "right before publish, steering_angle: ", error
steering_angle.publish(car_msg)
def timerCallBack(self,event):
state=self.path[self.state]
steer_angle = -1.*state[3]
vel = self.EXT_VEL
newControl = AckermannDriveStamped()
newDrive = AckermannDrive()
newDrive.steering_angle = steer_angle
newDrive.speed = vel
self.newControl.drive = newDrive
self.state+=1
def control(self,data):
car_ctl = AckermannDrive()
car_msg = AckermannDriveStamped()
object_angle=data.data[0]*math.pi/180
object_distance=data.data[1]
if data.data[0] != -1:
desired = -.3 #how wide to clear cone
multiplier=1
if(not self.right):
desired = .3
desired_distance=.1
distance_y = object_distance * math.cos(object_angle)
if distance_y < .4:
desired *= 2./3
steering_gain=.1
steering_max = .34
actual=multiplier * math.sin(object_angle)*object_distance
#if actual < desired and object_distance > desired * 1.2:
# actual = object_distance
error=desired-actual
print actual, " actual"
print object_distance, " object_distance"
print steering_gain * error, " steering"
steering_angle = multiplier*steering_gain * error
car_max_speed=1.0
#desired_distance=.1
#distance_y = object_distance * math.cos(object_angle)
distance_error=distance_y-desired_distance
print 'distance y ', distance_y
speed_gain=.4
speed=distance_error*speed_gain
print 'speed ', speed
#speed=.3
car_ctl.speed = speed
car_ctl.steering_angle = steering_angle
car_msg.header.stamp = rospy.Time.now()
car_msg.header.frame_id = "IPSController"
car_msg.drive = car_ctl
self.control_pub.publish(car_msg)
else:
car_ctl.steering_angle = 0
car_ctl.speed = 0 #change for, later crawl forward
car_msg.header.frame_id = "IPSController"
car_msg.drive = car_ctl
self.control_pub.publish(car_msg)
def control(self,data):
car_ctl = AckermannDrive()
car_msg = AckermannDriveStamped()
[centerX,centerY,width,height,actual_angle]=data.data
if actual_angle != -1 and width < 180 and height < 250:
desired_angle = 0
steering_gain=.4
steering_range = .34
error = desired_angle - actual_angle
steering_angle = steering_gain * error
car_ctl.speed = 0.3
car_ctl.steering_angle = steering_angle
car_msg.header.stamp = rospy.Time.now()
car_msg.header.frame_id = "IBSController"
car_msg.drive = car_ctl
self.control_pub.publish(car_msg)
else:
car_ctl.speed = 0.0
car_msg.header.frame_id = "IBSController"
car_msg.drive = car_ctl
self.control_pub.publish(car_msg)
def apply_control(self, speed, steering_angle):
self.actual_speed = speed
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = speed
drive_msg.steering_angle = steering_angle
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.control_pub.publish(drive_msg_stamped)
def otherCode(self, data):
pubAcker = rospy.Publisher('/autodrive', AckermannDrive,
queue_size=10) # init publisher
ack_msg = AckermannDrive() # initialize ackermann message
i = 0
speed = 1 #setting the speed
totalDist = [] #Setting arrays
angs = [] #for the angles that are road
while i < len(data.ranges):
totalDist.append(data.ranges[i])
if totalDist[i] > 1000000:
totalDist[i] = 100000
if totalDist[i] > 5 and totalDist[
i] < 8: # is the point in the road?
angs.append(math.radians(
(i - 380.0) * (190.0 / 760.0))) #log the angle (radians)
i += 1
ack_msg.speed = speed # set speed to the ackermann message
ack_msg.steering_angle = median(
angs) # set the angle to the ackermann message
pubAcker.publish(ack_msg)
rospy.loginfo(median(angs))
def callback(data):
global speed
global steering_angle
global distance_set
global acceleration
distance_set = [get_range(data, i) for i in range(90, -91, -1)]
steering_angle = 0.0
acceleration = 0.0
perception(data)
if simulator_mode == True:
drive_msg = AckermannDrive(steering_angle=steering_angle,
speed=speed,
acceleration=acceleration)
drive_st_msg = AckermannDriveStamped(drive=drive_msg)
drive_pub_simulator.publish(drive_st_msg)
else:
drive_msg = AckermannDrive(steering_angle=steering_angle,
speed=speed,
acceleration=acceleration)
drive_pub.publish(drive_msg)
def callback(self,data):
#dict with key = button, value = input
try:
#raises gamepad map format error
buttons = getButtons(data, self.gamepad)
#convert button input to driving commands, etc
commands = self.converter.getDriveCommands(buttons)
js_ret = commands['journey_start']
newSpeed = commands['speed']
newAngle = commands['angle']
deadMansSwitch = commands['dms']
autoCtrl = commands['auto_mode']
if js_ret:
js_msg = Bool()
js_msg.data = True
self.journeyStartPublisher.publish(js_msg)
dms = Bool()
dms.data = deadMansSwitch
self.dmsPublisher.publish(dms)
ac = Bool()
ac.data = autoCtrl
self.autoCtrlPublisher.publish(ac)
#only publish if needed
if deadMansSwitch and (not autoCtrl):
ack = AckermannDrive()
ack.steering_angle = newAngle
ack.speed = newSpeed
self.manualDrivePublisher.publish(ack)
except GamepadMapFormatError:
rospy.logfatal("%s, shutting down", GamepadMapFormatError.message)
exit(0)
def stop(self):
print "Stopping"
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0
drive_msg.steering_angle = 0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.control_pub.publish(drive_msg_stamped)
def send_command(pub_controls, c):
cmd = c.split(',')
assert len(cmd) == 2
v, delta = [float(d) for d in cmd]
dur = rospy.Duration(1.0)
rate = rospy.Rate(10)
start = rospy.Time.now()
drive = AckermannDrive(steering_angle=delta, speed=v)
while (rospy.Time.now() - start) < dur:
pub_controls.publish(AckermannDriveStamped(drive=drive))
rate.sleep()
def pursuit(self):
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
print 'robot_pose:', self.robot_pose, 'waypoints:', self.waypoints
self.destination_pose = self.circleIntersect(self.lookahead_distance)
print 'destination_pose:', self.destination_pose
print 'waypoint_index:', self.current_waypoint_index
if (self.destination_pose == None):
self.speed = 0
self.steering_angle = 0
else:
self.speed = self.default_speed
distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose)
angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose)
self.steering_angle = np.arctan((2 * self.robot_length * np.sin(angle_to_destination)) / distance_to_destination)
drive_msg.speed = self.speed
drive_msg.steering_angle = self.steering_angle
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
r.sleep()
def run(self, hz):
cmd_publisher = rospy.Publisher("/ackermann_cmd",
AckermannDrive,
queue_size=1)
rate = rospy.Rate(hz) # 30 Hz
screen = pygame.display.set_mode((1, 1))
# Mapping for PS4 Controller
# Axis 0 - Left Stick X
# Axis 1 - Left Stick Y
# Axis 2 - Left "Trigger"
# Axis 3 - Right Stick X
# Axis 4 - Right Stick Y
# Axis 5 - Right "Trigger"
while not rospy.is_shutdown():
# This has to be called at the start of each loop in order for pygame
# to update the axis status
pygame.event.get()
# Create a new "empty" message
msg = AckermannDrive()
print(msg.steering_angle)
print(msg.speed)
msg.steering_angle = -self.joy.get_axis(0)
l_t = self.joy.get_axis(2)
r_t = self.joy.get_axis(5)
msg.speed = 2.0 * ((r_t + 1.0) - (l_t + 1.0))
print("steering: {} speed: {}".format(msg.steering_angle,
msg.speed))
cmd_publisher.publish(msg)
rate.sleep()
def update(self):
"""
Processes gps data and waypoint and publishes new steering angle.
"""
# Double check there is data from fix and from the Hemisphere.
if nav_data.longitude is None or nav_data.direction is None:
return
# Update class variables.
self.current_longitude = nav_data.longitude
self.current_latitude = nav_data.latitude
self.current_angle = nav_data.direciton
# Figure out how to proceed if the tractorhas not reached.
if (isclose(self.current_latitude, self.waypoint_latitude) and \
isclose(self.current_longitude, self.waypoint_longitude)):
desired_angle = deg_calculate_desired_angle(self.current_latitude,\
self.current_longitude, self.waypoint_latitude, \
self.waypoint_longitude)
current_error = deg_calculate_steering_error(self.current_angle, \
desired_angle)
new_steering_angle = deg_calculate_steering_angle(current_error, \
self.kp1)
new_velocity = deg_calculate_forward_velocity(new_steering_angle, \
self.kp3)
# Create and publish a drive message.
drive_msg = AckermannDrive()
drive_msg.header.stamp = rospy.Time.now()
drive_msg.steering_angle = new_steering_angle
drive_msg.speed = abs(new_velocity)
self.pub.publish(drive_msg)
# Otherwise, we're ready to move on to the next waypoint.
else:
self.waypoint_reached = True
return
def apply_control(self, speed, steering_angle):
self.actual_speed = speed
drive_msg_stamped = AckermannDriveStamped()
drive_msg_stamped.header = utils.make_header("/base_link")
drive_msg = AckermannDrive()
drive_msg.speed = speed
# 正为左转，负为右转
drive_msg.steering_angle = steering_angle
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.control_pub.publish(drive_msg_stamped)
def publish_(self):
self._controller()
ai = self.throttle
di = self.steering
# print "throttle and steering angle :: ",di
print "speed and steering angle :: ", ai, di
print "\n\n"
ackermann_cmd_msg = AckermannDrive()
ackermann_cmd_msg.steering_angle = di
ackermann_cmd_msg.speed = ai
self.cmd_pub.publish(ackermann_cmd_msg)
# throttle_msg = make_throttle_msg(ai)
# steering_msg = make_steering_msg(di)
# brake_msg = make_brake_msg()
# self.throttle_pub.publish(throttle_msg)
# self.steering_pub.publish(steering_msg)
# self.brake_pub.publish(brake_msg)
rospy.on_shutdown(self.stopOnShutdown)
def step(self, action):
if rospy.is_shutdown():
raise (KeyboardInterrupt)
self._resumeGazebo()
ackermann_cmd = AckermannDrive()
ackermann_cmd.speed = action[0] * (MAX_SPEED - MIN_SPEED)
ackermann_cmd.steering_angle = action[1] * MAX_ANGLE
self.ackermann_pub.publish(ackermann_cmd)
rospy.sleep(self.step_size)
data = None
obs = None
done = False
while data is None and not rospy.is_shutdown():
try:
# data = rospy.wait_for_message('/ackermann_vehicle/camera1/image_raw', Image, timeout=5)
while self.image_msg is None:
rospy.logwarn_throttle(5, 'No camera updates in step')
data = self.image_msg
obs = self._imageMsgToCv2(data)
obs = cv2.cvtColor(obs, cv2.COLOR_BGR2RGB)
obs = np.float32(obs) / 255
self.image_msg = None
except:
rospy.logerr_throttle(1, 'No data found')
self._pauseGazebo()
state = obs
reward, done = self._rewardCalc()
return state, reward, done, {}
def control(data): global prev_error global vel_input global kp global kd global angle error = data.pid_error angle_correction = kp * error + (kd * (error - prev_error)) angle = - angle_correction print angle if angle > 100: angle = 100 elif angle < -100: angle = -100 angle = angle / 100 vel_input = 1 - abs(angle) if vel_input < 0.5: vel_input = 0.5 msg = AckermannDrive(); msg.speed = vel_input msg.steering_angle = angle pub.publish(msg)
def __init__(self, joyListener):
smach.State.__init__(self,
outcomes=['playback', 'stop'],
input_keys=['input'],
output_keys=['output'])
self.joyListener = joyListener
# Create an event to block on until we transition to the next state
self.stateChangeEvent = threading.Event()
self.car_ctl = AckermannDrive()
self.car_msg = AckermannDriveStamped()
self.cmdPub = rospy.Publisher('ackermann_cmd',
AckermannDriveStamped,
queue_size=1)
def callback(data):
speed = data.linear.x
turn = data.angular.z
msg = AckermannDrive()
msg.speed = speed
msg.acceleration = 1
msg.jerk = 1
msg.steering_angle = turn
msg.steering_angle_velocity = 1
pub.publish(msg)
def callback(data):
global speed
global steering_angle
global distance_set
distance_set = [get_range(data, i) for i in range(90, -91, -1)]
set_speedmode(4)
wall_following(distance_apart=1.0)
distant_narrow_front_perception()
near_wide_front_perception()
wheel()
# steering_angle = 0 radian in range [-0.4189, 0.4189]
drive_msg = AckermannDrive(steering_angle=steering_angle, speed=speed)
drive_st_msg = AckermannDriveStamped(drive=drive_msg)
drive_pub.publish(drive_st_msg)
def drive_straight(self):
while not rospy.is_shutdown():
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.5
drive_msg.steering_angle = 0.0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
# don't spin too fast
rospy.sleep(1.0/PUBLISH_RATE)
def callback(data):
rospy.loginfo(str(data.steer) + " - " + str(data.throttle))
ackermsg = AckermannDriveStamped()
ackermsg.header = Header()
ackermsg.header.stamp = rospy.Time.now()
ackermsg.drive = AckermannDrive()
ackermsg.drive.steering_angle = data.steer
ackermsg.drive.steering_angle_velocity = steering_angle_velocity
ackermsg.drive.speed = data.throttle
ackermsg.drive.acceleration = acceleration
ackermsg.drive.jerk = jerk
publisher.publish(ackermsg)
def drive(self, tags_tan):
turning_factor = self.get_turning_factor(tags_tan)
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = MAX_SPEED - abs(turning_factor / 2)
drive_msg.steering_angle = self.get_steering_angle(turning_factor)
drive_msg.acceleration = 0.0
drive_msg.jerk = 0.0
drive_msg.steering_angle_velocity = 0.1
drive_msg_stamped.drive = drive_msg
self.previous_steering_angle = drive_msg.steering_angle
self.pub.publish(drive_msg_stamped)
def execute(self, currentPose, referencePose):
# Compute the control input to the vehicle according to the
# current and reference pose of the vehicle
# Input:
# currentPose: ModelState, the current state of the vehicle
# referencePose: list, the reference state of the vehicle,
# the element in the list are [ref_x, ref_y, ref_theta, ref_v]
# Output: None
# TODO: Implement this function
#Pack computed velocity and steering angle into Ackermann command
newAckermannCmd = AckermannDrive()
# Publish the computed control input to vehicle model
self.controlPub.publish(newAckermannCmd)
def main():
model = AckermannMotionModel()
pathgen = AckermannPathGen(model)
rospy.init_node('ackermann_test')
freq = 16.
rate = rospy.Rate(1 / freq)
cmd = AckermannDrive(speed=1.0, steering_angle=0.3)
pose_sub = rospy.Subscriber('/car/particle_filter/inferred_pose',
PoseStamped, pathgen.handle_pose)
path_pub = rospy.Publisher('/car/planner/path', PoseArray, queue_size=1)
while not rospy.is_shutdown():
path_pub.publish(pathgen.get_path())
rate.sleep()
def __init__(self, data, pilotMode, vesc):
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive() #self.drive_msg = AckermannDrive()
if data[1]!='zed' and data[1]!='lidar':
#self.both(data[0], data[1])
logicData = [self.zedData, self.lidarData]
elif data[1]=='zed':
self.zed(data[0][0], data[0][1], data[0][2], data[0][3])
logicData = [self.zedData, 'zed']
elif data[1]=='lidar':
self.lidar(data[0][0], data[0][1], data[0][2], data[0][3], data[0][4], data[0][5])
logicData = [self.lidarData, 'lidar']
Logic(logicData, drive_msg, pilotMode)
drive_msg_stamped.drive = drive_msg
vesc.publish(drive_msg_stamped)
def driveTest():
rp.init_node("pleaseWork",anonymous = False)
pub=rp.Publisher("/vesc/ackermann_cmd_mux/input/navigation",AckermannDriveStamped,queue_size=10)
rate=rp.Rate(60)
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.8
drive_msg.steering_angle = 1.0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
while True:
pub.publish(drive_msg_stamped)
rate.sleep()
def __init__(self):
self.ackermann_cmd = AckermannDrive()
self.ackermann_cmd.speed = 1
self.ackermann_cmd.steering_angle = 0.0
self.ackermann_pub = rospy.Publisher('/ackermann_cmd',
AckermannDrive,
queue_size=1)
self.time_sub = rospy.Subscriber('/clock', Clock, self._clock_callback)
self.bumper_sub = rospy.Subscriber('/bumper_sensor', ContactsState,
self._bumper_callback)
self.drive_cur_time = 0
self.drive_init_time = 0
self.rate = rospy.Rate(10)
def tape_present(self, msg):
frame = self.bridge.imgmsg_to_cv2(msg, "mono8")
white_pixels = cv2.countNonZero(frame)
if white_pixels < MIN_PIXELS_TO_DRIVE:
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.0
drive_msg.steering_angle = 0.0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
def __init__(self):
# node setup
rospy.init_node("person_follower", anonymous=True)
# state from subscribers
self.tracker = PersonTracker()
self.right_bumper = False
self.last_bounding_box_timestamp = 0
self.last_speed = 0
self.last_angle = 0
# subscriber setup
sub = message_filters.Subscriber('/darknet_ros/bounding_boxes',
BoundingBoxes)
depth_sub = rospy.Subscriber('/camera/depth/image_rect_raw', Image,
self.tracker.depth_cb)
joy_sub = rospy.Subscriber('/vesc/joy', Joy, self.right_bump)
img_sub = message_filters.Subscriber('/camera/color/image_raw', Image)
ts = message_filters.ApproximateTimeSynchronizer([sub, img_sub], 10,
0.1)
sub.registerCallback(self.tracker.bounding_box_cb)
ts.registerCallback(self.tracker.img_cb)
# publisher setup
pub = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/input/teleop',
AckermannDriveStamped,
queue_size=10)
seq = 0
rate = rospy.Rate(30)
while not rospy.is_shutdown():
seq += 1
header = Header(seq=seq, stamp=rospy.Time.now())
if self.tracker.predicted_bounding_box is not None:
self.tracker.tracker_pub.publish(
self.tracker.predicted_bounding_box)
if self.tracker.average_tracked is not None:
self.tracker.average_img_pub.publish(
br.cv2_to_imgmsg(self.tracker.average_tracked))
angle, speed = self.get_steering_direction()
if self.right_bumper and angle and speed:
pub.publish(
AckermannDriveStamped(
header,
AckermannDrive(steering_angle=angle, speed=speed)))
rate.sleep()
rospy.spin()
def __init__(self):
IMAGE_WIDTH = 640
IMAGE_HEIGHT = 480
SERVO_RANGE = 0.34
MAX_PERCENT_OF_FRAME = 0.27
MIN_PERCENT_OF_FRAME = 0.18
self.last_box = None
self.person_bounding_box = None
self.right_bumper = False
# node setup
rospy.init_node("person_follower", anonymous=True)
sub = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes,
lambda x: self.cb(x))
joy_sub = rospy.Subscriber('/vesc/joy', Joy, self.right_bump)
pub = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/input/teleop',
AckermannDriveStamped,
queue_size=10)
rate = rospy.Rate(30)
seq = 0
while not rospy.is_shutdown():
seq += 1
header = Header(seq=seq, stamp=rospy.Time.now())
bb = self.person_bounding_box
if bb is not None:
boxCenter = (bb.xmin + bb.xmax) / 2.0
ratio = boxCenter / 640.0
angle = -1 * (-SERVO_RANGE + ratio * 2 * SERVO_RANGE)
percent_of_frame = (abs(bb.xmax - bb.xmin) *
abs(bb.ymax - bb.ymin)) / (640.0 * 480.0)
speed = 0
if percent_of_frame > MAX_PERCENT_OF_FRAME:
speed = -0.5
angle = -angle
elif percent_of_frame < MIN_PERCENT_OF_FRAME:
speed = 0.5
#print("angle is", angle, "speed is", speed, "percent of frame is", percent_of_frame)
if self.right_bumper:
pub.publish(
AckermannDriveStamped(
header,
AckermannDrive(steering_angle=angle, speed=speed)))
rate.sleep()
rospy.spin()
def publish_stop_message(self):
header = Header(
stamp=Time.now(),
frame_id="base_link",
)
drive = AckermannDrive(
steering_angle=self.last_steering_angle,
steering_angle_velocity=0,
speed=0, # STOP!
acceleration=0,
jerk=0,
)
msg = AckermannDriveStamped(
header=header,
drive=drive,
)
rospy.logwarn("STOP: safety controller ordered a stop!")
self.safety_pub.publish(msg)
def __init__(self):
super(RosThreaddedNode, self).__init__()
# Initialize the ros node
self.rosNode = rospy.init_node("autocar_keyop")
# Publisher
self.ackermannPub = rospy.Publisher("ackermann_cmd",
AckermannDrive,
queue_size=10)
self.currentAckermannMsg = AckermannDrive()
# self.jointStateSub = rospy.Subscriber("joint_states", JointState, self.onJointStateReceived)
# Joint states
self.currentSpeed = 0.0
self.currentSteerAngle = 0.0
# The scale value that decided how much the velocity will be increased on each keypress
self.speedScale = 0.1
self.angleScale = 0.01
# Collect parameters for parameter server
self.getParameters()
def __init__(self):
self.path_points = []
self._i = 0
self.final_goal = 0
self._sub = rospy.Subscriber("/ada/fix", NavSatFix, self.call_gps)
self._path = rospy.wait_for_message("route_points", Path)
self.pub = rospy.Publisher('ackermann_cmd',
AckermannDrive,
queue_size=10,
latch=True)
self.ackermann_msg = AckermannDrive()
self.state_pub = rospy.Publisher('state',
Float64,
queue_size=10,
latch=True)
self.control_effort = rospy.Subscriber('control_effort', Float64,
self.control_callback)
self.goal_sub = rospy.Subscriber("ada/goal_status", GoalStatusArray,
self.call_goal)
self.lateral_offset = 0.0
self.prev_lateral_offset = 0.0
self.yaw_error = 0.0
self.Ld = 0.0
self.Rd = 0.0
self.dt = 0.01
self.error = 0.0
self.yaw_angle_vehicle = 0.0
self.distance = 0.0
self.i = 0
self.current_pose = []
self.prev_pose = []
self.road_slope = None
self.direction = 0.0
self.prev_error = 0.0
self.path_points = []
self.Velocity_x = 10
