cleaned up

src/esw/imu_driver.py

@@ -82,9 +82,15 @@ def main():
     rospy.init_node("imu_driver")
 
     # read serial connection info and IMU frame from parameter server
+<<<<<<< Updated upstream
     port = rospy.get_param("imu_driver/port")
     baud = rospy.get_param("imu_driver/baud", 115200)
     imu_frame = rospy.get_param("imu_driver/frame_id", "imu_link")
+=======
+    port = "/dev/ttyACM0"
+    baud = 115200
+    imu_frame = "imu_link"
+>>>>>>> Stashed changes
 
     # create serial connection with Arduino
     ser = serial.Serial(port, baud)
@@ -162,7 +168,11 @@ def main():
 
         temp_msg = Temperature(header=header, temperature=temp_data)
 
+<<<<<<< Updated upstream
         # calibration_msg = CalibrationStatus(header, cal_data)
+=======
+        calibration_msg = CalibrationStatus(header, cal_data)
+>>>>>>> Stashed changes
 
         # publish each message
         imu_pub.publish(imu_msg)

src/localization/tip_detection.py

@@ -1,26 +1,16 @@
 #!/usr/bin/env python3
 
-from pathlib import Path
-from typing import Optional
 import tf2_ros
-from sensor_msgs.msg import NavSatFix, Imu
-import message_filters
 import numpy as np
 import rospy
-from geometry_msgs.msg import Twist
-from mrover.msg import MotorsStatus
-from nav_msgs.msg import Odometry
-from pandas import DataFrame
 from std_msgs.msg import Bool
-from util.ros_utils import get_rosparam
-from nav_msgs.msg import Odometry
 from util.SE3 import SE3
 import time
 
 
 class TipDetection:
     hit_count: int
-    z_orientation_threshold: float
+    orientation_threshold: float
     hit_count_threshold: int
     reset_hit_count_threshold: int
     time_counter: time
@@ -31,7 +21,7 @@ def __init__(self):
         self.tip_publisher = rospy.Publisher("tipping", Bool, queue_size=1)
 
         self.hit_count = 0
-        self.z_orientation_threshold = 0.8
+        self.orientation_threshold = 0.8
         self.hit_count_threshold = 10
         self.reset_hit_count_threshold = 3
         self.time_counter = time.time()
@@ -47,44 +37,42 @@ def in_loop(self):
         rate = rospy.Rate(10.0)
         while not rospy.is_shutdown():
             try:
-                # extract the  matrix by [0,0,1] (z vector)
+                # extract yaw
                 self.old = SE3.from_tf_tree(self.buffer, self.world_frame, self.rover_frame).rotation.rotation_matrix()
-                # multiply this transform by the z vector [0, 0, 1]
+
+                # 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
-                print(self.transform[2])
-                if self.transform[2] <= self.z_orientation_threshold:
+                if self.transform[2] <= self.orientation_threshold:
                     self.hit_count += 1
-                    self.check_for_hit_count(self.hit_count)
+
+                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 
             self.current_time = time.time()
             self.reset_hit_count_time(self.reset_hit_count_threshold, self.time_counter)
 
-    # seeing if hit_count is too big
+    # check if hit_count is too big
     def check_for_hit_count(self, hit_count):
+        # if hit_count exceeds threshold
         if hit_count > self.hit_count_threshold:
-            rospy.loginfo("tipping")
-            rospy.loginfo(hit_count)
             # publishing into tip_publisher that rover is tipping, True
             self.tip_publisher.publish(True)
-        else: # else print False
+        else: # else publish False
             self.tip_publisher.publish(False)
 
-    # resetting hit_count each reset_hit_count_threshold seconds
+    # reset hit_count each reset_hit_count_threshold seconds
     def reset_hit_count_time(self, reset_hit_count_threshold, time_counter):
-        print(f"time {time.time()- self.time_counter}")
+        # if the amount of time that's passed since last reset > threshold, reset hit_count
         if time.time() - self.time_counter > self.reset_hit_count_threshold:
-            rospy.loginfo("resetting hit count...")
             self.hit_count = 0
             self.time_counter = time.time()
 
 def main():
-    # rospy.loginfo("===== tip detection starting =====")
     rospy.init_node("tip_detection")
     TipDetection()