def __init__(self):
# velocity publisher
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# lidar listener
self.scan_listener = LidarListener()
# angular and linear velocity in rescue mode
self.angular_vel = 0.2 #rad/s
self.linear_vel = 1.0 #m/s
# threshold showing there exists obstacles
self.stop_threshold = 1.5 # if any thing within this range emerge in the laser scan, stop robot
self.move_threshold = 4.0 # if all points in the front are farther then this value, go straight forward
# parameter storing the width of the robot
self.robot_width = 1.5
self.open_area_pts = 40 # at least 40 points exist in an "open area"
#self.out_of_boundary_threshold = 5 # threshold for tolerating small number of points within move_threshold
# paramter storing the current mode of the robot
self.mode = "turning" # valid values: "turning", "forward"
# define the variable to have velocity
self.twist = Twist()
# store the previous turning state
self.previous_turn = None
self.cumulative_turn_angle = 0.0 # store the cumulative turning angle
self.view_coef = 0.5 # coefficient to determine the view for navigation detection
self.view_coef_reset = 0.5 # backup copy of the coefficient to determine the view for navigation detection
self.view_decay_rate = 0.5 # gradually narrow the view
self.straight_count = 0
self.restore_view_threshold = 10 # move straight 5 steps and then recover the view
