def main():
rospy.init_node("line_follower") #initialize ros node
print("starting line follower")
rate = rospy.Rate(30)
# Instantiating classes
fetcher = LineDataFetcher() # fetches line data
publisher = VESCPublisher() # publishes control command to VESC
line_follower = LineFollower(
) # passed line data thru PID controller to generate the control signal
heading_ctrl = PID_ctrl(Kp = heading_ctrl_Kp, Ki = 0., Kd = 0., \
i_max = 0., i_min = 0., \
ctrl_max = heading_ctrl_ctrl_max, ctrl_min = heading_ctrl_ctrl_min) # control based on line angle
lateral_ctrl = PID_ctrl(Kp = lateral_ctrl_Kp, Ki = lateral_ctrl_Ki, Kd = lateral_ctrl_Kd, \
i_max = lateral_ctrl_i_max, i_min = lateral_ctrl_i_min, \
ctrl_max = lateral_ctrl_ctrl_max, ctrl_min = lateral_ctrl_ctrl_min) # control based on lateral offset
# Initializations
timestamp = time.time()
heading_ctrl.t_prev = timestamp
heading_ctrl.t_curr = timestamp
lateral_ctrl.t_prev = timestamp
lateral_ctrl.t_curr = timestamp
while not rospy.is_shutdown():
timestamp = time.time()
line_data = fetcher.get_linedata() # fetch line data
line_follower.import_linedata(
line_data) #, trav_dist) # import line data to line follower
line_follower.calculate_ctrl(
heading_ctrl, lateral_ctrl,
timestamp) # calculate the steering control
line_follower.set_servo_control()
motor, servo = line_follower.get_ECU_data()
servo_reg = servo / 180 * pi
servo_reg = min(max(servo_reg, -0.34), 0.34)
motor_reg = min(max(motor, -2.0), 2.0)
print("Servo", servo)
print("Servo_reg", servo_reg)
print("Motor", motor)
print("Motor_reg", motor_reg)
print("t", timestamp)
ads_msg = ADS()
ads_msg.header.seq += 1
ads_msg.header.stamp.secs = timestamp
ads_msg.drive.steering_angle = servo_reg
ads_msg.drive.speed = motor_reg
publisher.pub_vesc_msg(ads_msg)
rate.sleep()
def __init__(self):
self.VESC_msg = ADS()
self.VESC_pub = rospy.Publisher(
"/vesc/low_level/ackermann_cmd_mux/input/navigation",
ADS,
queue_size=1)
def main():
rospy.init_node("HallNavwithObjDet")
rate = rospy.Rate(40)
servo = 0.
motor = 0.
control = PID_ctrl(Kp = Kp, Ki = Ki, Kd = Kd, \
i_max = i_max, i_min = i_min, \
ctrl_max = ctrl_max, ctrl_min = ctrl_min) # PID controller
fetcher = LidarFetcher() # gets lidar data
publisher = VESCPublisher() # publishes control command to VESC
timestamp = time.time()
lidardata = fetcher.get_lidardata()
while lidardata == []: # while lidar data has not been populated
print("Waiting for Lidar data to populate")
lidardata = fetcher.get_lidardata() # update lidardata
# get lidar characteristics
ang_min = lidardata.angle_min / pi * 180 # minimum lidar scan angle in degrees
ang_max = lidardata.angle_max / pi * 180 # maximum lidar scan angle in degrees
ang_step = lidardata.angle_increment / pi *180 # lidar angle increment
zedfetcher = ZEDFetcher()
detection = zedfetcher.get_zed_data()
detectedtime = timestamp
print("1")
while not rospy.is_shutdown():
print("1.5")
timestamp = time.time()
detection = zedfetcher.get_zed_data()
detected = False
if detection == []: # check if lidar data is populated
continue
for i in detection.bounding_boxes:
if (i.Class in objects):
print("DETECTED ", i.Class, "at ", timestamp)
detected = True
break
print("2")
ads_msg = ADS()
lidardata = fetcher.get_lidardata() # get lidar data
if lidardata == []: # check if lidar data is populated
continue # if not populated --> skip to next iteration
right_dist = lidardata.ranges[180] # extract right distance
front_dist = lidardata.ranges[int(540 - col_ang*4):int(540 + col_ang*4 + 1)] # extract a range of front measurements
left_dist = lidardata.ranges[900] # extract left distance
print('right', right_dist)
print('front', front_dist[int(len(front_dist)/2)])
print('left', left_dist)
#print('all', type(lidardata.ranges))
# check if any of the front distance measurements are below a certain threshold
front_blocked = False
min_front = min(front_dist)
print('min_front', min_front)
if min_front < front_thresh:
front_blocked = True
print("3")
if min_front < abs_stop_thresh:
#motor = 0
if lidardata.ranges[540] < max_lidar_range:
goBack(init_front_dist = lidardata.ranges[540], rev_dist= reverse_dist, rate = rate, \
ads_msg = ads_msg, fetcher = fetcher, publisher = publisher)
print("Too close to obstacle ahead. Cannot move")
elif (detected and (timestamp - detectedtime) > 11.0):
print("Im not going to drive past the stop sign")
detectedtime = timestamp
print("DETECTED at ", timestamp)
for i in range(stoptime):
timestamp = time.time()
ads_msg.header.seq+=1
ads_msg.header.stamp.secs = timestamp
ads_msg.drive.steering_angle = 0
ads_msg.drive.speed = 0
publisher.pub_vesc_msg(ads_msg) # publish the message
rate.sleep() # sleep until next iteration
elif (front_blocked): # check if front is blocked
if(left_dist < left_thresh): # check if left is blocked
if(right_dist < right_thresh): # check if right is blocked
# this is a deadend
motor = 0.
print("deadend")
else:
# blocked ahead but opening on right side (hallway on right side)
# sharp right turn
motor = default_turn
servo = MAX_servo
print("rightturn")
else:
# blocked ahead but opening on left side (hallway on left side)
# sharp left turn
motor = default_turn
servo = MIN_servo
print("leftturn")
else:
if(left_dist < left_thresh): # check if there is a close wall/object to the left
# nothing ahead + wall to left
# regulate left distance through PID
motor = default_forward
servo = - control.apply_pid(des_value = PID_thresh, current_value = left_dist, timestamp = timestamp)
print("PID control")
else:
# nothing ahead + opening to the left (hallway on left side)
# sharp left turn
motor = default_turn
servo = MIN_servo
print("leftturn")
print("4")
# populate the message to send to the ESC
ads_msg.header.seq+=1
ads_msg.header.stamp.secs = timestamp
ads_msg.drive.steering_angle = servo
ads_msg.drive.speed = motor
publisher.pub_vesc_msg(ads_msg) # publish the message
rate.sleep() # sleep until next iteration