weird code
fixed boolean logics
code style
fixed many bugs introduced by mixing ros time with system time