def stop_car():
pub = rospy.Publisher('drive_parameters', drive_params, queue_size=5)
for i in range(50):
msg = drive_params()
msg.angle = 0
msg.velocity = 0
pub.publish(msg)
def lidar_callback(self, lidar_data):
""" This is asynchronously called every time we receive new LIDAR data.
"""
self.total_packets += 1
# If the lock is currently locked, then previous LIDAR data is still
# being processed.
if not self.lock.acquire(False):
self.dropped_packets += 1
return
if self.should_stop:
return
start_time = time.time()
distances = lidar_data.ranges
self.lidar_distances = distances
self.samples_per_degree = float(len(distances)) / self.scan_width
target_distance, target_angle = self.find_new_angle()
safe_distances = self.masked_disparities
forward_distance = safe_distances[len(safe_distances) / 2]
#target_angle = self.adjust_angle_to_avoid_uturn(target_angle,
# forward_distance)
target_angle = self.adjust_angle_for_car_side(target_angle)
desired_speed = self.duty_cycle_from_distance(forward_distance)
self.update_considered_angle(target_angle)
steering_percentage = self.degrees_to_steering_percentage(target_angle)
msg = drive_params()
msg.angle = steering_percentage
msg.velocity = desired_speed
self.pub_drive_param.publish(msg)
duration = time.time() - start_time
self.lock.release()
print "(took %.02f ms): Target point %.02fm at %.02f degrees" % (
duration * 1000.0, target_distance, target_angle)
def callback(data):
print "\t inside callback"
global line_control
global DriveParamPublisher
msg = drive_params()
msg.velocity = data.velocity
msg.angle = data.angle
print line_control
if line_control == True:
print "\t\t inside line control"
DriveParamPublisher.publish(msg)
def lidar_callback(self, lidar_data):
""" This is asynchronously called every time we receive new LIDAR data.
"""
# If the lock is currently locked, then previous LIDAR data is still
# being processed.
if not self.lock.acquire(False):
return
if self.should_stop:
return
target_velocity = self.velocity
distances = lidar_data.ranges
forward_distance = distances[len(distances) / 2]
current_time = time.time()
# If the car just started moving, record the overall start time
if self.start_time is None:
self.start_time = current_time
# If the acceleration duration has elapsed, then record the time and
# distance at which we'll start the speed calculation.
if (self.start_distance is None
) and current_time >= (self.start_time + self.accelerate_duration):
self.accelerate_end_time = current_time
self.start_distance = forward_distance
# If the measurement duration has elapsed, then calculate the speed
# based on the distance that we moved.
if (self.accelerate_end_time is not None) and (
current_time >=
(self.accelerate_end_time + self.measurement_duration)):
self.should_stop = True
target_velocity = 0.0
time_elapsed = current_time - self.accelerate_end_time
distance_elapsed = self.start_distance - forward_distance
speed = distance_elapsed / time_elapsed
print "Current distance: %f. Traveled %fm in %fs. Speed: %f m/s" % (
forward_distance, distance_elapsed, time_elapsed, speed)
msg = drive_params()
msg.angle = 0.5
msg.velocity = target_velocity
if forward_distance < 3.0:
msg.velocity = 0.0
self.pub_drive_param.publish(msg)
self.lock.release()
def main():
print("Geometric Controller Initialized")
rospy.init_node('geometric_controller_filter')
pub = rospy.Publisher("drive_parameters", drive_params)
while not rospy.core.is_shutdown():
msg = rospy.client.wait_for_message('scan', LaserScan)
print(msg.angle_min, msg.angle_max, msg.angle_increment)
TOP = 1
BOT = 0
thetas = 3 * math.pi / 24
thetar = [-math.pi / 2, (-math.pi / 2) + thetas]
rangesr = [get_range(msg, t) for t in thetar]
pointsr = [(rangesr[BOT], 0), get_coords(thetar[TOP], rangesr[TOP])]
thetasr = get_steering_angle(pointsr[TOP], pointsr[BOT])
print('pointsr', pointsr)
print('thetasR', thetasr)
thetal = [math.pi / 2, (math.pi / 2) - thetas]
rangesl = [get_range(msg, t) for t in thetal]
pointsl = [(-rangesl[BOT], 0), get_coords(thetal[TOP], rangesl[TOP])]
thetasl = get_steering_angle(pointsl[TOP], pointsl[BOT])
print('pointsl', pointsl)
print('thetasL', thetasl)
"""
Weight the thetas by how far the max distance is to the wall.
This should keep us away from walls.
"""
maxr = max(rangesr)**2
maxl = max(rangesl)**2
rweight = maxr / (maxr + maxl)
lweight = maxl / (maxr + maxl)
print("Weights:", lweight, "|", rweight)
thetas = thetasl * lweight + thetasr * rweight
steering_input = interp(
thetas, [np.radians(-20), np.radians(20)], [-100, 100])
print("ThetaR:", thetas, "|Steering Angle:", steering_input)
message = drive_params()
message.angle = steering_input
message.velocity = 15
pub.publish(message)
#! /usr/bin/env python
import rospy
from ac_msgs.msg import drive_params
from std_msgs.msg import Bool
import time
rospy.init_node("BugControl")
DriveParamPublisher = rospy.Publisher("drive_parameters", drive_params, queue_size=10)
EStopPublisher = rospy.Publisher("eStop", Bool, queue_size=10)
msg = drive_params()
while(1):
#EStopPublisher.publish(False)
for a in range 100:
time.sleep(0.01)
turn = (a / 100)
msg.angle = turn
msg.velocity = 0
DriveParamPublisher.publish(msg)
for b in range 100:
time.sleep(0.01)
turn = 1 - (b / 100)
msg.angle = turn
msg.velocity = 0
DriveParamPublisher.publish(msg)
continue
