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