def scan_callback(self, scan_msg):
# TODO: calculate TTC
# pdb.set_trace()
speed = self.speed
if speed != 0:
ranges = np.array(scan_msg.ranges)
angles = np.arange(scan_msg.angle_min, scan_msg.angle_max,
scan_msg.angle_increment)
sin_angles = np.where(angles != 0, abs(np.sin(angles)),
0.000000001)
# calculate the max lengths of interest -- for those that are too far away, mark them as
# infinite
if speed > 0:
max_ranges = np.where(
(angles > -np.pi / 2.) & (angles < np.pi / 2.),
car_width / (2. * sin_angles), 0)
else:
# car is moving backwards, look at the other half of the ranges
max_ranges = np.where(
(angles < -np.pi / 2.) | (angles > np.pi / 2.),
car_width / (2. * sin_angles), 0)
speed = abs(speed)
diffs = max_ranges - ranges
# the only angles we care about are those that are positive, but not nan
mask = (diffs > 0) & (~np.isnan(diffs)) & (~np.isinf(diffs))
ttc = np.min(
np.where(mask,
ranges * abs(np.cos(angles)) / speed,
np.inf)).astype(float)
else:
ttc = np.inf
speed = 0
ttc_threshold = speed / car_deceleration
if ttc < ttc_threshold:
if not self.brake_engaged:
rospy.loginfo(
"Engaging Brake! Current Speed: %f, minTTC: %f, threshold: %f",
self.speed, ttc, ttc_threshold)
self.brake_engaged = True
msg = AckermannDriveStamped()
msg.header.seq = self.seq
self.seq += 1
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = ""
msg.drive.speed = 0
msg.drive.steering_angle = 0
msg.drive.steering_angle_velocity = 0
msg.drive.acceleration = 0
msg.drive.jerk = 0
self.ebrake_publisher.publish(msg)
else:
self.brake_engaged = False
msg = Bool()
msg.data = self.brake_engaged
self.ebrake_state_publisher.publish(msg)
