Skip to content

Commit

Permalink
Format
Browse files Browse the repository at this point in the history
  • Loading branch information
qhdwight committed Apr 19, 2024
1 parent 73c9320 commit 7d8ac9a
Show file tree
Hide file tree
Showing 2 changed files with 6 additions and 6 deletions.
2 changes: 1 addition & 1 deletion src/esw/imu_driver.py
Original file line number Diff line number Diff line change
Expand Up @@ -172,4 +172,4 @@ def main():


if __name__ == "__main__":
main()
main()
10 changes: 5 additions & 5 deletions src/localization/tip_detection.py
Original file line number Diff line number Diff line change
Expand Up @@ -40,20 +40,20 @@ def in_loop(self):
try:
# extract yaw
self.old = SE3.from_tf_tree(self.buffer, self.world_frame, self.rover_frame).rotation.rotation_matrix()

# multiply yaw by the z vector [0, 0, 1] to get new transform
self.transform = np.dot(np.array([0, 0, 1]), self.old)

# compare this new transform with our threshold to see if it's tipping, if so increment hit_count
if self.transform[2] <= self.orientation_threshold:
self.hit_count += 1

self.check_for_hit_count(self.hit_count)

except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
print(e)
rate.sleep()
# reset the hit count time
# reset the hit count time
self.current_time = time.time()
self.reset_hit_count_time(self.reset_hit_count_threshold, self.time_counter)

Expand All @@ -63,7 +63,7 @@ def check_for_hit_count(self, hit_count):
if hit_count > self.hit_count_threshold:
# publishing into tip_publisher that rover is tipping, True
self.tip_publisher.publish(True)
else: # else publish False
else: # else publish False
self.tip_publisher.publish(False)

# reset hit_count each reset_hit_count_threshold seconds
Expand All @@ -73,6 +73,7 @@ def reset_hit_count_time(self, reset_hit_count_threshold, time_counter):
self.hit_count = 0
self.time_counter = time.time()


def main():
rospy.init_node("tip_detection")
TipDetection()
Expand All @@ -82,4 +83,3 @@ def main():

if __name__ == "__main__":
main()

0 comments on commit 7d8ac9a

Please sign in to comment.