From c317c1e54f7e375cd424b70b378a8cf9c1ab1fcb Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Tue, 31 Oct 2023 18:33:10 -0400
Subject: [PATCH 01/24] Add starter project files

---
 .../autonomy/AutonomyStarterProject.cmake     |  2 +-
 .../autonomy/config/starter_project.world     |  2 +-
 .../autonomy/launch/starter_project.launch    |  2 +-
 .../autonomy/msg/StarterProjectTag.msg        |  4 ++
 starter_project/autonomy/src/localization.py  | 14 ++++
 .../autonomy/src/navigation/context.py        | 15 +++--
 .../autonomy/src/navigation/drive_state.py    | 14 +++-
 .../navigation/navigation_starter_project.py  | 12 ++++
 .../autonomy/src/navigation/tag_seek.py       | 14 +++-
 starter_project/autonomy/src/perception.cpp   | 64 +++++++++++++++++--
 10 files changed, 128 insertions(+), 15 deletions(-)
 create mode 100644 starter_project/autonomy/msg/StarterProjectTag.msg

diff --git a/starter_project/autonomy/AutonomyStarterProject.cmake b/starter_project/autonomy/AutonomyStarterProject.cmake
index 4b84c75af..d8873b08a 100644
--- a/starter_project/autonomy/AutonomyStarterProject.cmake
+++ b/starter_project/autonomy/AutonomyStarterProject.cmake
@@ -8,7 +8,7 @@ add_message_files(
         DIRECTORY
         ${CMAKE_CURRENT_LIST_DIR}/msg
         FILES
-        #add new messages here
+        StarterProjectTag.msg
 )
 
 # Collect all cpp files in the src subdirectory to be used for perception
diff --git a/starter_project/autonomy/config/starter_project.world b/starter_project/autonomy/config/starter_project.world
index a8235fb42..e9c8644f9 100644
--- a/starter_project/autonomy/config/starter_project.world
+++ b/starter_project/autonomy/config/starter_project.world
@@ -40,7 +40,7 @@
       <heading_deg>0</heading_deg>
     </spherical_coordinates>
     <model name='waypoint_post'>
-      <pose>5.5 2.0 0.5 0 0 4.71239</pose>
+      <pose>1.0 0.0 0.5 0 0 4.71239</pose>
       <static>1</static>
       <link name='link'>
         <collision name='collision'>
diff --git a/starter_project/autonomy/launch/starter_project.launch b/starter_project/autonomy/launch/starter_project.launch
index a0b8ce28c..f84576947 100644
--- a/starter_project/autonomy/launch/starter_project.launch
+++ b/starter_project/autonomy/launch/starter_project.launch
@@ -30,7 +30,7 @@ starter_project/
     Perception
     ==========
   -->
-     <node name="starter_project_perception" pkg="mrover" type="starter_project_perception" />
+     <!-- <node name="starter_project_perception" pkg="mrover" type="starter_project_perception" /> -->
 
   <!--
     ===========
diff --git a/starter_project/autonomy/msg/StarterProjectTag.msg b/starter_project/autonomy/msg/StarterProjectTag.msg
new file mode 100644
index 000000000..fb3b6f843
--- /dev/null
+++ b/starter_project/autonomy/msg/StarterProjectTag.msg
@@ -0,0 +1,4 @@
+int32 tagId
+float32 xTagCenterPixel
+float32 yTagCenterPixel
+float32 closenessMetric
\ No newline at end of file
diff --git a/starter_project/autonomy/src/localization.py b/starter_project/autonomy/src/localization.py
index b8ff5423a..d34365801 100755
--- a/starter_project/autonomy/src/localization.py
+++ b/starter_project/autonomy/src/localization.py
@@ -20,6 +20,8 @@ class Localization:
     def __init__(self):
         # create subscribers for GPS and IMU data, linking them to our callback functions
         # TODO
+        rospy.Subscriber("imu/imu_only", Imu, self.imu_callback)
+        rospy.Subscriber("gps/fix", NavSatFix, self.gps_callback)
 
         # create a transform broadcaster for publishing to the TF tree
         self.tf_broadcaster = tf2_ros.TransformBroadcaster()
@@ -35,6 +37,11 @@ def gps_callback(self, msg: NavSatFix):
         that pose to the TF tree.
         """
         # TODO
+        ref = np.array([(42.293195), (-83.7096706)])
+        point = np.array([(msg.latitude), (msg.longitude)])
+        cartesian = self.spherical_to_cartesian(point, ref)
+        self.pose = SE3(cartesian, self.pose.rotation)
+        self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
 
     def imu_callback(self, msg: Imu):
         """
@@ -43,6 +50,8 @@ def imu_callback(self, msg: Imu):
         store that value in `self.pose`, then publish that pose to the TF tree.
         """
         # TODO
+        self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
+        self.pose.publish_to_tf_tree(sefl.tf_broadcaster, "map", "base_link")
 
     @staticmethod
     def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndarray) -> np.ndarray:
@@ -57,6 +66,11 @@ def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndar
         :returns: the approximated cartesian coordinates in meters, given as a numpy array [x, y, z]
         """
         # TODO
+        r = 6371000
+        x = (r* (np.radians(spherical_coord[1]) - np.radians(reference_coord[1])) * np.cos(np.radians(reference_coord[0])))
+        y = r * (np.radians(spherical_coord[0]) - np.radians(reference_coord[0]))
+        z = 0
+        return np.array([x, y, z])
 
 
 def main():
diff --git a/starter_project/autonomy/src/navigation/context.py b/starter_project/autonomy/src/navigation/context.py
index 4bbd06f61..b679a2ac6 100644
--- a/starter_project/autonomy/src/navigation/context.py
+++ b/starter_project/autonomy/src/navigation/context.py
@@ -18,15 +18,18 @@ class Rover:
 
     def get_pose(self) -> Optional[SE3]:
         # TODO: return the pose of the rover (or None if we don't have one (catch exception))
-        pass
+        try:
+            return SE3.SE3.from_tf_tree(self.ctx.tf_buffer, "map", "base_link")
+        except:
+            return None
 
     def send_drive_command(self, twist: Twist):
         # TODO: send the Twist message to the rover
-        pass
+        self.ctx.vel_cmd_publisher.publish(twist)
 
     def send_drive_stop(self):
         # TODO: tell the rover to stop
-        pass
+        self.send_drive_command(Twist())
 
 
 @dataclass
@@ -41,7 +44,7 @@ class Environment:
 
     def receive_fid_data(self, message: StarterProjectTag):
         # TODO: handle incoming FID data messages here
-        pass
+        self.fid_pos = message
 
     def get_fid_data(self) -> Optional[StarterProjectTag]:
         """
@@ -49,7 +52,9 @@ def get_fid_data(self) -> Optional[StarterProjectTag]:
         if it exists, otherwise returns None
         """
         # TODO: return either None or your position message
-        pass
+        if (self.fid_pos) {
+            return self.fid_pos
+        } else {return None}
 
 
 class Context:
diff --git a/starter_project/autonomy/src/navigation/drive_state.py b/starter_project/autonomy/src/navigation/drive_state.py
index bae1d07f1..3c35f2efc 100644
--- a/starter_project/autonomy/src/navigation/drive_state.py
+++ b/starter_project/autonomy/src/navigation/drive_state.py
@@ -10,21 +10,31 @@ def __init__(self, context: Context):
         super().__init__(
             context,
             # TODO:
-            add_outcomes=["TODO: add outcomes here"],
+            add_outcomes=["driving_to_point", "reached_point"],
         )
 
     def evaluate(self, ud):
         target = np.array([5.5, 2.0, 0.0])
 
         # TODO: get the rover's pose, if it doesn't exist stay in DriveState (with outcome "driving_to_point")
-
+        SE3_pose = self.context.rover.get_pose()
+        if SE3_pose is None:
+            return "driving_to_point
+            "
         # TODO: get the drive command and completion status based on target and pose
         # (HINT: use get_drive_command(), with completion_thresh set to 0.7 and turn_in_place_thresh set to 0.2)
+        drive_command, completion_status = get_drive_command(
+            target_pos = target, rover_pose = SE3_pose, completion_thresh = 0.7, turn_in_place_thresh = 0.2
+        )
 
         # TODO: if we are finished getting to the target, go to TagSeekState (with outcome "reached_point")
+        if completion_status:
+            return "reached_point"
 
         # TODO: send the drive command to the rover
+        self.context.rover.send_drive_command(drive_command)
 
         # TODO: tell smach to stay in the DriveState by returning with outcome "driving_to_point"
+        return "driving_to_point"
 
         pass
diff --git a/starter_project/autonomy/src/navigation/navigation_starter_project.py b/starter_project/autonomy/src/navigation/navigation_starter_project.py
index a3f00f0dd..1bbedeed7 100755
--- a/starter_project/autonomy/src/navigation/navigation_starter_project.py
+++ b/starter_project/autonomy/src/navigation/navigation_starter_project.py
@@ -30,6 +30,11 @@ def __init__(self, context: Context):
         self.sis.start()
         with self.state_machine:
             # TODO: add DriveState and its transitions here
+            self.state_machine.add(
+                "DriveState",
+                DriveState(self.context),
+                transitions={"driving_to_point": "DriveState", "reached_point": "TagSeekState"},
+            )
 
             # DoneState and its transitions
             self.state_machine.add(
@@ -38,6 +43,12 @@ def __init__(self, context: Context):
                 transitions={"done": "DoneState"},
             )
             # TODO: add TagSeekState and its transitions here
+            self.state_machine.add(
+                "TagSeekState",
+                TagSeekState(self.context),
+                transitions={"failure": "DoneState", "working": "TagSeekState", "success": "DoneState"},
+            )
+
 
     def run(self):
         self.state_machine.execute()
@@ -53,6 +64,7 @@ def stop(self):
 
 def main():
     # TODO: init a node called "navigation"
+    rospy.init_node("navigation")
 
     # context and navigation objects
     context = Context()
diff --git a/starter_project/autonomy/src/navigation/tag_seek.py b/starter_project/autonomy/src/navigation/tag_seek.py
index 577731516..1254e1935 100644
--- a/starter_project/autonomy/src/navigation/tag_seek.py
+++ b/starter_project/autonomy/src/navigation/tag_seek.py
@@ -9,22 +9,34 @@ def __init__(self, context: Context):
         super().__init__(
             context,
             # TODO: add outcomes
-            add_outcomes=["TODO: add outcomes here"],
+            add_outcomes=["failure", "working", "success"],
         )
 
     def evaluate(self, ud):
         DISTANCE_TOLERANCE = 0.99
         ANUGLAR_TOLERANCE = 0.3
         # TODO: get the tag's location and properties (HINT: use get_fid_data() from context.env)
+        tag = self.context.env.get_fid_data()
 
         # TODO: if we don't have a tag: go to the DoneState (with outcome "failure")
+        if tag is None:
+            return "failure"
 
         # TODO: if we are within angular and distance tolerances: go to DoneState (with outcome "success")
+        if tag.closenessMetric < DISTANCE_TOLERANCE and abs(tag.xTagCenterPixel) < ANUGLAR_TOLERANCE:
+            return "success"
 
         # TODO: figure out the Twist command to be applied to move the rover closer to the tag
+        twist = Twist()
+        if tag.closenessMetric >= DISTANCE_TOLERANCE:
+            twist.linear.x = 1
+        if abs(tag.closenessMetric) >= ANUGLAR_TOLERANCE:
+            twist.angular.z = tag.xTagCenterPixel
 
         # TODO: send Twist command to rover
+        self.context.rover.send_drive_command(twist)
 
         # TODO: stay in the TagSeekState (with outcome "working")
+        return "working"
 
         pass
diff --git a/starter_project/autonomy/src/perception.cpp b/starter_project/autonomy/src/perception.cpp
index 2185a5fe2..8f05bd9b5 100644
--- a/starter_project/autonomy/src/perception.cpp
+++ b/starter_project/autonomy/src/perception.cpp
@@ -26,7 +26,7 @@ namespace mrover {
         // Create a publisher for our tag topic
         // See: http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
         // TODO: uncomment me!
-        // mTagPublisher = mNodeHandle.advertise<StarterProjectTag>("tag", 1);
+        mTagPublisher = mNodeHandle.advertise<StarterProjectTag>("tag", 1);
 
         mTagDetectorParams = cv::aruco::DetectorParameters::create();
         mTagDictionary = cv::aruco::getPredefinedDictionary(0);
@@ -54,15 +54,49 @@ namespace mrover {
         tags.clear(); // Clear old tags in output vector
 
         // TODO: implement me!
+        cv::aruco::detectMarkers(image, mTagDictionary, mTagCorners, mTagIds, mTagDetectorParams);
+
+        for (size_t i = 0; i < mTagIds.size(); ++i) {
+            StarterProjectTag tag;
+            tag.tagId = mTagIds[i];
+            std::pair<float, float> center = getCenterFromTagCorners(mTagCorners[i]);
+
+            tag.xTagCenterPixel = (center.first - image.cols/2) / image.cols;
+            tag.yTagCenterPixel = (center.second - image.rows/2) / image.rows;
+            tag.closenessMetric = getClosenessMetricFromTagCorners(image, mTagCorners[i]);
+            mTags.push_back(tag);
+        }
+        
     }
 
     StarterProjectTag Perception::selectTag(std::vector<StarterProjectTag> const& tags) { // NOLINT(*-convert-member-functions-to-static)
         // TODO: implement me!
-        return {};
+        if (tags.empty()) {
+            StarterProjectTag tag;
+            tag.tagId = -1;
+            return tag;
+        } else if (tags.size() == 1) { 
+            return tags[0];
+        } else {
+            int minTagIndex = 0;
+            double minTagDist = sqrt(pow(tags[0].xTagCenterPixel, 2) + pow(tags[0].yTagCenterPixel,2));
+
+            for (size_t i = 1; i < tags.size(); ++i) {
+                double currentTagDist = sqrt(pow(tags[i].xTagCenterPixel, 2) + pow(tags[i].yTagCenterPixel,2));
+
+                if (currentTagDist < minTagDist) {
+                    minTagDist = currentTagDist;
+                    minTagIndex = i;
+                }
+            }
+            
+            return tags[minTagIndex];
+        } 
     }
 
     void Perception::publishTag(StarterProjectTag const& tag) {
         // TODO: implement me!
+        mTagPublisher.publish(tag);
     }
 
     float Perception::getClosenessMetricFromTagCorners(cv::Mat const& image, std::vector<cv::Point2f> const& tagCorners) { // NOLINT(*-convert-member-functions-to-static)
@@ -71,12 +105,34 @@ namespace mrover {
         // hint: try not overthink, this metric does not have to be perfectly accurate, just correlated to distance away from a tag
 
         // TODO: implement me!
-        return {};
+        float areaImage = image.rows * image.cols;
+        cv::Point2f topLeft = tagCorners[0];
+        cv::Point2f topRight = tagCorners[1];
+        cv::Point2f bottomLeft = tagCorners[2];
+
+        float tagWidth = topRight.x - topLeft.x;
+        float tagHeight = bottomLeft.y - topLeft.y;
+        ROS_INFO("tag width: %f tag height: %f", tagWidth, tagHeight);
+
+        float ratio = 1 - (abs(tagWidth * tagHeight) / areaImage);
+
+        return ratio;
     }
 
     std::pair<float, float> Perception::getCenterFromTagCorners(std::vector<cv::Point2f> const& tagCorners) { // NOLINT(*-convert-member-functions-to-static)
         // TODO: implement me!
-        return {};
+        int xSum = 0;
+        int ySum = 0;
+
+        for (cv::Point2f corner : tagCorners) {
+            xSum += corner.x;
+            ySum += corner.y;
+        }
+
+        std::pair<float, float> center;
+        center.first = xSum;
+        center.second = ySum;
+        return center;
     }
 
 } // namespace mrover

From cbdfcae579c82ad52ca74728f8a720eac354226a Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Thu, 30 Nov 2023 19:09:59 -0500
Subject: [PATCH 02/24] getting started

git push
---
 src/localization/tip_detection.py | 76 +++++++++++++++++++++++++++++++
 1 file changed, 76 insertions(+)
 create mode 100644 src/localization/tip_detection.py

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
new file mode 100644
index 000000000..e1e021102
--- /dev/null
+++ b/src/localization/tip_detection.py
@@ -0,0 +1,76 @@
+#!/usr/bin/env python3
+
+from pathlib import Path
+from typing import Optional
+
+import message_filters
+import numpy as np
+import pandas as pd
+import rospy
+from geometry_msgs.msg import Twist
+from mrover.msg import MotorsStatus
+from nav_msgs.msg import Odometry
+from pandas import DataFrame
+from smach_msgs.msg import SmachContainerStatus
+from std_msgs.msg import Bool
+from util.ros_utils import get_rosparam
+
+# detect what the pitch is like in the function
+# if the pitch is too high, make it a fail state
+# do fail states later? 
+
+class tip_detection: 
+    def __init__(self):
+        rospy.Subscriber("imu/imu_only", Imu, self.detect_tip)
+        #read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
+    def imu_callback(self, msg: Imu):
+        self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
+        self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
+        odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
+        
+
+        #subscribe to IMU
+    def detect_tip(self, odometry):
+
+        # get rover position
+        cur_row = {}
+        cur_row["x"] = odometry.pose.pose.position.x
+        cur_row["y"] = odometry.pose.pose.position.y
+        cur_row["z"] = odometry.pose.pose.position.z
+
+        # get rover orientation
+
+        cur_row["rot_x"] = odometry.pose.pose.orientation.x
+        cur_row["rot_y"] = odometry.pose.pose.orientation.y
+        cur_row["rot_z"] = odometry.pose.pose.orientation.z
+        cur_row["rot_w"] = odometry.pose.pose.orientation.w
+
+        # get rover linear acceleration and velocity
+        linear_velocity_norm = np.linalg.norm(
+            np.array([odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z])
+        )
+
+        cur_row["linear_velocity"] = linear_velocity_norm  # type: ignore
+        cur_row["angular_velocity"] = odometry.twist.twist.angular.z
+        
+
+
+
+        
+        """"
+        quartonions:
+        basically weird way to store the way the rotations are rotating
+        qx = ax * sin(angle/2)
+        qy = ay * sin(angle/2)
+        qz = az * sin(angle/2)
+        qw = cos(angle/2)
+
+        ax, ay, and az are how much it is angled off of each axis
+        phone rotating on table: 0, 0, 1
+        w is how much it's being rotated based on x,y,z
+
+        when talking ab multiplying quartonions, it's just adding them
+        one 90 deg rotate + one 90 deg rotate = 180 deg rotate
+
+        
+        """"

From f3c96d98da25d785aa6aef04b40fe562e801a975 Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Thu, 30 Nov 2023 19:43:28 -0500
Subject: [PATCH 03/24] planning

---
 src/localization/tip_detection.py | 47 ++++++++++++++++++-------------
 1 file changed, 28 insertions(+), 19 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index e1e021102..f3282cd37 100644
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -23,41 +23,52 @@ class tip_detection:
     def __init__(self):
         rospy.Subscriber("imu/imu_only", Imu, self.detect_tip)
         #read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
+
     def imu_callback(self, msg: Imu):
         self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
         self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
         odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
         
-
         #subscribe to IMU
+        
     def detect_tip(self, odometry):
-
-        # get rover position
-        cur_row = {}
-        cur_row["x"] = odometry.pose.pose.position.x
-        cur_row["y"] = odometry.pose.pose.position.y
-        cur_row["z"] = odometry.pose.pose.position.z
-
         # get rover orientation
 
-        cur_row["rot_x"] = odometry.pose.pose.orientation.x
-        cur_row["rot_y"] = odometry.pose.pose.orientation.y
-        cur_row["rot_z"] = odometry.pose.pose.orientation.z
-        cur_row["rot_w"] = odometry.pose.pose.orientation.w
+        x = odometry.pose.pose.orientation.x
+        y = odometry.pose.pose.orientation.y
+        z = odometry.pose.pose.orientation.z
+        w = odometry.pose.pose.orientation.w
 
         # get rover linear acceleration and velocity
         linear_velocity_norm = np.linalg.norm(
             np.array([odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z])
         )
 
-        cur_row["linear_velocity"] = linear_velocity_norm  # type: ignore
-        cur_row["angular_velocity"] = odometry.twist.twist.angular.z
-        
-
+        linear_velocity = linear_velocity_norm 
+        angular_velocity = odometry.twist.twist.angular.z
 
 
         
         """"
+        check if rover is tipped over over a certain period of time
+        if there are a lot of fluctuations over an arbitrary time period,
+        then the rover is tipped
+
+        ex. if there are 50 over-tipped measurements over 10 secs, then tipped
+        made up numbers ^^
+        
+
+        
+        ways we can check the cases for tipping:
+        - measure magnitudes of angular velocity and see if there is one that is above a threshold
+        - velocity is = or near 0 for a set amount of time
+            - check orientation
+        
+
+
+
+
+
         quartonions:
         basically weird way to store the way the rotations are rotating
         qx = ax * sin(angle/2)
@@ -71,6 +82,4 @@ def detect_tip(self, odometry):
 
         when talking ab multiplying quartonions, it's just adding them
         one 90 deg rotate + one 90 deg rotate = 180 deg rotate
-
-        
-        """"
+        """"
\ No newline at end of file

From 50cca37d48fa71a31f68d5db28d3febffff558b4 Mon Sep 17 00:00:00 2001
From: Neda Gulen <nedagln@umich.edu>
Date: Tue, 5 Dec 2023 23:58:16 +0000
Subject: [PATCH 04/24] first_check

---
 src/localization/tip_detection.py | 155 ++++++++++++++++++++----------
 1 file changed, 103 insertions(+), 52 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index f3282cd37..62a9f953f 100644
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -2,10 +2,10 @@
 
 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 pandas as pd
 import rospy
 from geometry_msgs.msg import Twist
 from mrover.msg import MotorsStatus
@@ -14,72 +14,123 @@
 from smach_msgs.msg import SmachContainerStatus
 from std_msgs.msg import Bool
 from util.ros_utils import get_rosparam
+from nav_msgs.msg import Odometry
+from util.SE3 import SE3
 
 # detect what the pitch is like in the function
 # if the pitch is too high, make it a fail state
-# do fail states later? 
+# do fail states later?
+
+class tip_detection:
+hit_count: int
+threshold: int
+angular_threshold: int
+linear_threshold: int
+
+def __init__(self):
+rospy.Subscriber("imu/imu_only", Imu, self.detect_tip)
+#read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
+self.hit_count = 0
+self.threshold = 0
+self.angular_threshold = 0
+self.linear_threshold = 0
+
+def imu_callback(self, msg: Imu):
+self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
+self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
+odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
+
+#subscribe to IMU
+
+def detect_tip(self, odometry):
+# get rover orientation
+
+x = odometry.pose.pose.orientation.x
+y = odometry.pose.pose.orientation.y
+z = odometry.pose.pose.orientation.z
+w = odometry.pose.pose.orientation.w
+
+
+# hit count set to different threshold values; if more than x values out of 10 point out to the rover tipping over; the rover is tipping over
+
+if w >= self.threshold or -self.threshold >= w:
+hit_count +=1
+elif x >= self.threshold or -self.threshold >= x:
+hit_count +=1
+elif y >= self.threshold or -self.threshold >= y:
+hit_count +=1
+
+def first_check(self, odometry, angular_velocity):
+# check magnitude of angular velocity for each axis
+x = odometry.pose.pose.orientation.x
+y = odometry.pose.pose.orientation.y
+z = odometry.pose.pose.orientation.z
+w = odometry.pose.pose.orientation.w
+
+# create the separate angular velocity variables
+angular_velocity_x = odometry.twist.twist.angular.x
+angular_velocity_y = odometry.twist.twist.angular.y
+
+# create the magnitude variables
+angular_velocity_x_magnitude = [angular_velocity_x]
+angular_velocity_y_magnitude = [angular_velocity_y]
+
+#check angular velocity magnitudes of each axis to see if it's above the threshold
+
+# Tipping over (pitch)
+if angular_velocity_x_magnitude > self.angular_threshold:
+hit_count += 1
+# Rolling over (roll)
+elif angular_velocity_y_magnitude > self.angular_threshold:
+hit_count += 1
+
+def second_check(self, odometry, linear_acceleration):
+x = odometry.pose.pose.orientation.x
+y = odometry.pose.pose.orientation.y
+z = odometry.pose.pose.orientation.z
+w = odometry.pose.pose.orientation.w
+
+
+
 
-class tip_detection: 
-    def __init__(self):
-        rospy.Subscriber("imu/imu_only", Imu, self.detect_tip)
-        #read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
 
-    def imu_callback(self, msg: Imu):
-        self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
-        self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
-        odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
-        
-        #subscribe to IMU
-        
-    def detect_tip(self, odometry):
-        # get rover orientation
 
-        x = odometry.pose.pose.orientation.x
-        y = odometry.pose.pose.orientation.y
-        z = odometry.pose.pose.orientation.z
-        w = odometry.pose.pose.orientation.w
 
-        # get rover linear acceleration and velocity
-        linear_velocity_norm = np.linalg.norm(
-            np.array([odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z])
-        )
+linear_velocity_norm = np.linalg.norm(
+np.array([odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z])
+)
 
-        linear_velocity = linear_velocity_norm 
-        angular_velocity = odometry.twist.twist.angular.z
+linear_velocity = linear_velocity_norm
 
 
-        
-        """"
-        check if rover is tipped over over a certain period of time
-        if there are a lot of fluctuations over an arbitrary time period,
-        then the rover is tipped
 
-        ex. if there are 50 over-tipped measurements over 10 secs, then tipped
-        made up numbers ^^
-        
+""""
+check if rover is tipped over over a certain period of time
+if there are a lot of fluctuations over an arbitrary time period,
+then the rover is tipped
 
-        
-        ways we can check the cases for tipping:
-        - measure magnitudes of angular velocity and see if there is one that is above a threshold
-        - velocity is = or near 0 for a set amount of time
-            - check orientation
-        
 
+ex. if there are 50 over-tipped measurements over 10 secs, then tipped
+made up numbers ^^
 
 
 
+ways we can check the cases for tipping:
+- measure magnitudes of angular velocity and see if there is one that is above a threshold
+- velocity is = or near 0 for a set amount of time
+- check orientation
 
-        quartonions:
-        basically weird way to store the way the rotations are rotating
-        qx = ax * sin(angle/2)
-        qy = ay * sin(angle/2)
-        qz = az * sin(angle/2)
-        qw = cos(angle/2)
+quartonions:
+basically weird way to store the way the rotations are rotating
+qx = ax * sin(angle/2)
+qy = ay * sin(angle/2)
+qz = az * sin(angle/2)
+qw = cos(angle/2)
 
-        ax, ay, and az are how much it is angled off of each axis
-        phone rotating on table: 0, 0, 1
-        w is how much it's being rotated based on x,y,z
+ax, ay, and az are how much it is angled off of each axis
+phone rotating on table: 0, 0, 1
+w is how much it's being rotated based on x,y,z
 
-        when talking ab multiplying quartonions, it's just adding them
-        one 90 deg rotate + one 90 deg rotate = 180 deg rotate
-        """"
\ No newline at end of file
+when talking ab multiplying quartonions, it's just adding them
+one 90 deg rotate + one 90 deg rotate = 180 deg rotate
+""""
\ No newline at end of file

From a32a41d561b3d9e36b9b4672bf7f769603f61615 Mon Sep 17 00:00:00 2001
From: Neda Gulen <nedagln@umich.edu>
Date: Wed, 6 Dec 2023 00:51:42 +0000
Subject: [PATCH 05/24] second_check

---
 src/localization/tip_detection.py | 247 ++++++++++++++++--------------
 1 file changed, 135 insertions(+), 112 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index 62a9f953f..657da15ec 100644
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -22,115 +22,138 @@
 # do fail states later?
 
 class tip_detection:
-hit_count: int
-threshold: int
-angular_threshold: int
-linear_threshold: int
-
-def __init__(self):
-rospy.Subscriber("imu/imu_only", Imu, self.detect_tip)
-#read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
-self.hit_count = 0
-self.threshold = 0
-self.angular_threshold = 0
-self.linear_threshold = 0
-
-def imu_callback(self, msg: Imu):
-self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
-self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
-odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
-
-#subscribe to IMU
-
-def detect_tip(self, odometry):
-# get rover orientation
-
-x = odometry.pose.pose.orientation.x
-y = odometry.pose.pose.orientation.y
-z = odometry.pose.pose.orientation.z
-w = odometry.pose.pose.orientation.w
-
-
-# hit count set to different threshold values; if more than x values out of 10 point out to the rover tipping over; the rover is tipping over
-
-if w >= self.threshold or -self.threshold >= w:
-hit_count +=1
-elif x >= self.threshold or -self.threshold >= x:
-hit_count +=1
-elif y >= self.threshold or -self.threshold >= y:
-hit_count +=1
-
-def first_check(self, odometry, angular_velocity):
-# check magnitude of angular velocity for each axis
-x = odometry.pose.pose.orientation.x
-y = odometry.pose.pose.orientation.y
-z = odometry.pose.pose.orientation.z
-w = odometry.pose.pose.orientation.w
-
-# create the separate angular velocity variables
-angular_velocity_x = odometry.twist.twist.angular.x
-angular_velocity_y = odometry.twist.twist.angular.y
-
-# create the magnitude variables
-angular_velocity_x_magnitude = [angular_velocity_x]
-angular_velocity_y_magnitude = [angular_velocity_y]
-
-#check angular velocity magnitudes of each axis to see if it's above the threshold
-
-# Tipping over (pitch)
-if angular_velocity_x_magnitude > self.angular_threshold:
-hit_count += 1
-# Rolling over (roll)
-elif angular_velocity_y_magnitude > self.angular_threshold:
-hit_count += 1
-
-def second_check(self, odometry, linear_acceleration):
-x = odometry.pose.pose.orientation.x
-y = odometry.pose.pose.orientation.y
-z = odometry.pose.pose.orientation.z
-w = odometry.pose.pose.orientation.w
-
-
-
-
-
-
-
-linear_velocity_norm = np.linalg.norm(
-np.array([odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z])
-)
-
-linear_velocity = linear_velocity_norm
-
-
-
-""""
-check if rover is tipped over over a certain period of time
-if there are a lot of fluctuations over an arbitrary time period,
-then the rover is tipped
-
-
-ex. if there are 50 over-tipped measurements over 10 secs, then tipped
-made up numbers ^^
-
-
-
-ways we can check the cases for tipping:
-- measure magnitudes of angular velocity and see if there is one that is above a threshold
-- velocity is = or near 0 for a set amount of time
-- check orientation
-
-quartonions:
-basically weird way to store the way the rotations are rotating
-qx = ax * sin(angle/2)
-qy = ay * sin(angle/2)
-qz = az * sin(angle/2)
-qw = cos(angle/2)
-
-ax, ay, and az are how much it is angled off of each axis
-phone rotating on table: 0, 0, 1
-w is how much it's being rotated based on x,y,z
-
-when talking ab multiplying quartonions, it's just adding them
-one 90 deg rotate + one 90 deg rotate = 180 deg rotate
-""""
\ No newline at end of file
+    hit_count: int
+    threshold: int
+    angular_threshold: int
+    linear_threshold: int
+    time_threshold: int
+
+    def __init__(self):
+        rospy.Subscriber("imu/imu_only", Imu, self.detect_tip)
+        #read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
+        self.hit_count = 0
+        self.threshold = 0
+        self.angular_threshold = 0
+        self.linear_threshold = 0
+        self.time_threshold = 0
+
+    def imu_callback(self, msg: Imu):
+        self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
+        self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
+        odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
+
+
+    def detect_tip(self, odometry):
+    # get rover orientation
+        x = odometry.pose.pose.orientation.x
+        y = odometry.pose.pose.orientation.y
+        z = odometry.pose.pose.orientation.z
+        w = odometry.pose.pose.orientation.w
+
+
+    # hit count set to different threshold values; if more than x values out of 10 point out to the rover tipping over; the rover is tipping over
+
+        if w >= self.threshold or -self.threshold >= w:
+            hit_count +=1
+        elif x >= self.threshold or -self.threshold >= x:
+            hit_count +=1
+        elif y >= self.threshold or -self.threshold >= y:
+            hit_count +=1
+
+    def first_check(self, odometry, angular_velocity):
+    # check magnitude of angular velocity for each axis
+        x = odometry.pose.pose.orientation.x
+        y = odometry.pose.pose.orientation.y
+        z = odometry.pose.pose.orientation.z
+        w = odometry.pose.pose.orientation.w
+
+    # create the separate angular velocity variables
+        angular_velocity_x = odometry.twist.twist.angular.x
+        angular_velocity_y = odometry.twist.twist.angular.y
+
+        # create the magnitude variables
+        angular_velocity_x_magnitude = [angular_velocity_x]
+        angular_velocity_y_magnitude = [angular_velocity_y]
+
+        #check angular velocity magnitudes of each axis to see if it's above the threshold
+
+    # Tipping over (pitch)
+        if angular_velocity_x_magnitude > self.angular_threshold:
+            hit_count += 1
+    # Rolling over (roll)
+        elif angular_velocity_y_magnitude > self.angular_threshold:
+            hit_count += 1
+
+    def perform_robotic_tasks(self,time_threshold):
+        # Simulate some robotic tasks
+        rospy.loginfo("Performing robotic tasks...")
+        rospy.sleep(self.time_threshold)  # Simulate a x second delay
+
+    def second_check(self, odometry, linear_acceleration):
+        x = odometry.pose.pose.orientation.x
+        y = odometry.pose.pose.orientation.y
+        z = odometry.pose.pose.orientation.z
+        w = odometry.pose.pose.orientation.w
+
+        linear_acceleration_norm = np.linalg.norm( np.array([odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z]))   
+        
+        rospy.init_node('elapsed_time_example_node', anonymous=True)
+        start_time = rospy.Time.now()
+        self.perform_robotic_tasks(self.time_threshold)
+        current_time = rospy.Time.now()
+        # Calculate the time difference
+        elapsed_time = current_time - start_time
+        # Extract the elapsed time in seconds
+        elapsed_seconds = elapsed_time.to_sec()
+
+        linear_acceleration_x = linear_acceleration_norm.x
+        linear_acceleration_y = linear_acceleration_norm.y
+
+        if linear_acceleration_x == 0 and elapsed_seconds > self.time_threshold:
+            if w >= self.threshold or -self.threshold >= w:
+                hit_count +=1
+            elif x >= self.threshold or -self.threshold >= x:
+                hit_count +=1
+            elif y >= self.threshold or -self.threshold >= y:
+                hit_count +=1
+        elif linear_acceleration_y == 0 and elapsed_seconds  > self.time_threshold:
+            if w >= self.threshold or -self.threshold >= w:
+                hit_count +=1
+            elif x >= self.threshold or -self.threshold >= x:
+                hit_count +=1
+            elif y >= self.threshold or -self.threshold >= y:
+                hit_count +=1
+
+
+
+
+    """"
+    check if rover is tipped over over a certain period of time
+    if there are a lot of fluctuations over an arbitrary time period,
+    then the rover is tipped
+
+
+    ex. if there are 50 over-tipped measurements over 10 secs, then tipped
+    made up numbers ^^
+
+
+
+    ways we can check the cases for tipping:
+    - measure magnitudes of angular velocity and see if there is one that is above a threshold
+    - velocity is = or near 0 for a set amount of time
+    - check orientation
+
+    quartonions:
+    basically weird way to store the way the rotations are rotating
+    qx = ax * sin(angle/2)
+    qy = ay * sin(angle/2)
+    qz = az * sin(angle/2)
+    qw = cos(angle/2)
+
+    ax, ay, and az are how much it is angled off of each axis
+    phone rotating on table: 0, 0, 1
+    w is how much it's being rotated based on x,y,z
+
+    when talking ab multiplying quartonions, it's just adding them
+    one 90 deg rotate + one 90 deg rotate = 180 deg rotate
+    """"
\ No newline at end of file

From 472bdb85c5f29a48cff1fca550e5e35af8c0d989 Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Thu, 7 Dec 2023 18:47:17 -0500
Subject: [PATCH 06/24] added main

---
 src/localization/tip_detection.py | 30 ++++++++++++++++++++++++------
 1 file changed, 24 insertions(+), 6 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index 657da15ec..1190ee671 100644
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -27,6 +27,7 @@ class tip_detection:
     angular_threshold: int
     linear_threshold: int
     time_threshold: int
+    hit_count_threshold: int
 
     def __init__(self):
         rospy.Subscriber("imu/imu_only", Imu, self.detect_tip)
@@ -36,6 +37,7 @@ def __init__(self):
         self.angular_threshold = 0
         self.linear_threshold = 0
         self.time_threshold = 0
+        hit_count_threshold = 0
 
     def imu_callback(self, msg: Imu):
         self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
@@ -50,7 +52,6 @@ def detect_tip(self, odometry):
         z = odometry.pose.pose.orientation.z
         w = odometry.pose.pose.orientation.w
 
-
     # hit count set to different threshold values; if more than x values out of 10 point out to the rover tipping over; the rover is tipping over
 
         if w >= self.threshold or -self.threshold >= w:
@@ -60,7 +61,8 @@ def detect_tip(self, odometry):
         elif y >= self.threshold or -self.threshold >= y:
             hit_count +=1
 
-    def first_check(self, odometry, angular_velocity):
+    # checking velocity of rover
+    def first_check(self, odometry):
     # check magnitude of angular velocity for each axis
         x = odometry.pose.pose.orientation.x
         y = odometry.pose.pose.orientation.y
@@ -84,12 +86,14 @@ def first_check(self, odometry, angular_velocity):
         elif angular_velocity_y_magnitude > self.angular_threshold:
             hit_count += 1
 
-    def perform_robotic_tasks(self,time_threshold):
-        # Simulate some robotic tasks
+    #crates the time difference
+    def perform_robotic_tasks(self, time_threshold):
+        # Simulate  robotic tasks
         rospy.loginfo("Performing robotic tasks...")
-        rospy.sleep(self.time_threshold)  # Simulate a x second delay
+        rospy.sleep(time_threshold)  # Simulate a x second delay
 
-    def second_check(self, odometry, linear_acceleration):
+    # checking acceleration of rover
+    def second_check(self, odometry):
         x = odometry.pose.pose.orientation.x
         y = odometry.pose.pose.orientation.y
         z = odometry.pose.pose.orientation.z
@@ -124,8 +128,22 @@ def second_check(self, odometry, linear_acceleration):
             elif y >= self.threshold or -self.threshold >= y:
                 hit_count +=1
 
+    # seeing if hit_count is too big
+    def check_for_hit_count(self, hit_count):
+        if hit_count > self.hit_count_threshold:
+            print("tipping")
+
 
+def main():
+    rospy.loginfo("===== tip detection starting =====")
+    rospy.init_node("tip_detection")
+    tip_detection()
+    rospy.spin() #idk what this is im copying from failure_identification
 
+    
+if __name__ == "__main__":
+    main()
+    
 
     """"
     check if rover is tipped over over a certain period of time

From 9e6b55b45026495e9656af7ffe2c08c5e832607b Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Thu, 7 Dec 2023 19:52:45 -0500
Subject: [PATCH 07/24] testing

---
 launch/auton.launch               |  1 +
 launch/auton_sim.launch           |  2 +-
 scripts/debug_course_publisher.py |  2 +-
 src/localization/tip_detection.py | 78 ++++++++++++++++---------------
 4 files changed, 43 insertions(+), 40 deletions(-)
 mode change 100644 => 100755 src/localization/tip_detection.py

diff --git a/launch/auton.launch b/launch/auton.launch
index 763657241..3d63c02b6 100644
--- a/launch/auton.launch
+++ b/launch/auton.launch
@@ -47,4 +47,5 @@
 
   <!-- node to linearize GPS from geodetic to ENU cartesian coords -->
   <node name="gps_linearization" pkg="mrover" type="gps_linearization.py" output="screen"/>
+  <node name="tip_detection" pkg="mrover" type="tip_detection.py" output="screen" />
 </launch>
diff --git a/launch/auton_sim.launch b/launch/auton_sim.launch
index 37cfaa6ea..49b224d10 100644
--- a/launch/auton_sim.launch
+++ b/launch/auton_sim.launch
@@ -3,7 +3,7 @@
  -->
 <launch>
     <arg name="rvizconfig" default="$(find mrover)/config/rviz/auton_sim.rviz"/>
-    <arg name="world_name" default="world_flat"/>
+    <arg name="world_name" default="world"/>
 
     <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true"/>
 
diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py
index 1c34e1bae..b70cf1d42 100755
--- a/scripts/debug_course_publisher.py
+++ b/scripts/debug_course_publisher.py
@@ -25,7 +25,7 @@
         (
             Waypoint(fiducial_id=0, tf_id="course0", type=WaypointType(val=WaypointType.NO_SEARCH)),
             # SE3(position=np.array([-844.37, 10351.56, 0])),
-            SE3(position=np.array([-8, -8, 0])),
+            SE3(position=np.array([15, -20, 0])),
         ),
         # (Waypoint(fiducial_id=0, tf_id="course1"), SE3(position=np.array([-3, -3, -1]))),
         # (Waypoint(fiducial_id=0, tf_id="course2"), SE3(position=np.array([-5, -5, 0]))),
diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
old mode 100644
new mode 100755
index 1190ee671..9760b8dc8
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -30,7 +30,7 @@ class tip_detection:
     hit_count_threshold: int
 
     def __init__(self):
-        rospy.Subscriber("imu/imu_only", Imu, self.detect_tip)
+        rospy.Subscriber("imu/imu_only", Imu, self.imu_callback)
         #read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
         self.hit_count = 0
         self.threshold = 0
@@ -38,53 +38,60 @@ def __init__(self):
         self.linear_threshold = 0
         self.time_threshold = 0
         hit_count_threshold = 0
+        odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
+        odometry_sub.registerCallback(self.odometry_callback)
+        self.x = 0
+        self.y = 0
+        self.z = 0
+        self.w = 0
 
     def imu_callback(self, msg: Imu):
-        self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
-        self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
-        odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
+        pass
+        # self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
+        # self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
+    
+    def odometry_callback(self, odometry):
+        self.x = abs(odometry.pose.pose.orientation.x)
+        self.y = abs(odometry.pose.pose.orientation.y)
+        self.z = abs(odometry.pose.pose.orientation.z)
+        self.w = abs(odometry.pose.pose.orientation.w)
 
 
     def detect_tip(self, odometry):
     # get rover orientation
-        x = odometry.pose.pose.orientation.x
-        y = odometry.pose.pose.orientation.y
-        z = odometry.pose.pose.orientation.z
-        w = odometry.pose.pose.orientation.w
+        print("x" + str(x))
+        print("y" + str(y))
+        print("z" + str(z))
+        print("w" + str(w))
 
     # hit count set to different threshold values; if more than x values out of 10 point out to the rover tipping over; the rover is tipping over
 
-        if w >= self.threshold or -self.threshold >= w:
-            hit_count +=1
-        elif x >= self.threshold or -self.threshold >= x:
-            hit_count +=1
-        elif y >= self.threshold or -self.threshold >= y:
-            hit_count +=1
+        if w >= self.threshold:
+            self.hit_count +=1
+        elif x >= self.threshold:
+            self.hit_count +=1
+        elif y >= self.threshold:
+            self.hit_count +=1
 
     # checking velocity of rover
-    def first_check(self, odometry):
     # check magnitude of angular velocity for each axis
-        x = odometry.pose.pose.orientation.x
-        y = odometry.pose.pose.orientation.y
-        z = odometry.pose.pose.orientation.z
-        w = odometry.pose.pose.orientation.w
 
     # create the separate angular velocity variables
         angular_velocity_x = odometry.twist.twist.angular.x
         angular_velocity_y = odometry.twist.twist.angular.y
 
         # create the magnitude variables
-        angular_velocity_x_magnitude = [angular_velocity_x]
-        angular_velocity_y_magnitude = [angular_velocity_y]
+        angular_velocity_x_magnitude = abs[angular_velocity_x]
+        angular_velocity_y_magnitude = abs[angular_velocity_y]
 
         #check angular velocity magnitudes of each axis to see if it's above the threshold
 
     # Tipping over (pitch)
         if angular_velocity_x_magnitude > self.angular_threshold:
-            hit_count += 1
+            self.hit_count += 1
     # Rolling over (roll)
         elif angular_velocity_y_magnitude > self.angular_threshold:
-            hit_count += 1
+            self.hit_count += 1
 
     #crates the time difference
     def perform_robotic_tasks(self, time_threshold):
@@ -94,11 +101,6 @@ def perform_robotic_tasks(self, time_threshold):
 
     # checking acceleration of rover
     def second_check(self, odometry):
-        x = odometry.pose.pose.orientation.x
-        y = odometry.pose.pose.orientation.y
-        z = odometry.pose.pose.orientation.z
-        w = odometry.pose.pose.orientation.w
-
         linear_acceleration_norm = np.linalg.norm( np.array([odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z]))   
         
         rospy.init_node('elapsed_time_example_node', anonymous=True)
@@ -110,22 +112,22 @@ def second_check(self, odometry):
         # Extract the elapsed time in seconds
         elapsed_seconds = elapsed_time.to_sec()
 
-        linear_acceleration_x = linear_acceleration_norm.x
-        linear_acceleration_y = linear_acceleration_norm.y
+        linear_acceleration_x = abs(linear_acceleration_norm.x)
+        linear_acceleration_y = abs(linear_acceleration_norm.y)
 
         if linear_acceleration_x == 0 and elapsed_seconds > self.time_threshold:
-            if w >= self.threshold or -self.threshold >= w:
+            if w >= self.threshold:
                 hit_count +=1
-            elif x >= self.threshold or -self.threshold >= x:
+            elif x >= self.threshold:
                 hit_count +=1
-            elif y >= self.threshold or -self.threshold >= y:
+            elif y >= self.threshold:
                 hit_count +=1
         elif linear_acceleration_y == 0 and elapsed_seconds  > self.time_threshold:
-            if w >= self.threshold or -self.threshold >= w:
+            if w >= self.threshold:
                 hit_count +=1
-            elif x >= self.threshold or -self.threshold >= x:
+            elif x >= self.threshold:
                 hit_count +=1
-            elif y >= self.threshold or -self.threshold >= y:
+            elif y >= self.threshold:
                 hit_count +=1
 
     # seeing if hit_count is too big
@@ -145,7 +147,7 @@ def main():
     main()
     
 
-    """"
+    """
     check if rover is tipped over over a certain period of time
     if there are a lot of fluctuations over an arbitrary time period,
     then the rover is tipped
@@ -174,4 +176,4 @@ def main():
 
     when talking ab multiplying quartonions, it's just adding them
     one 90 deg rotate + one 90 deg rotate = 180 deg rotate
-    """"
\ No newline at end of file
+    """
\ No newline at end of file

From 3ea3312cc70b4621f03a2b4e86916000b45ea242 Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Tue, 16 Jan 2024 19:37:28 -0500
Subject: [PATCH 08/24] debugging

---
 src/localization/tip_detection.py | 100 +++++++++++++++++-------------
 1 file changed, 56 insertions(+), 44 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index 9760b8dc8..740da8c57 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -21,7 +21,8 @@
 # if the pitch is too high, make it a fail state
 # do fail states later?
 
-class tip_detection:
+
+class TipDetection:
     hit_count: int
     threshold: int
     angular_threshold: int
@@ -31,13 +32,13 @@ class tip_detection:
 
     def __init__(self):
         rospy.Subscriber("imu/imu_only", Imu, self.imu_callback)
-        #read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
+        # read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
         self.hit_count = 0
         self.threshold = 0
         self.angular_threshold = 0
         self.linear_threshold = 0
         self.time_threshold = 0
-        hit_count_threshold = 0
+        self.hit_count_threshold = 0
         odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
         odometry_sub.registerCallback(self.odometry_callback)
         self.x = 0
@@ -49,34 +50,39 @@ def imu_callback(self, msg: Imu):
         pass
         # self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
         # self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
-    
+
     def odometry_callback(self, odometry):
         self.x = abs(odometry.pose.pose.orientation.x)
         self.y = abs(odometry.pose.pose.orientation.y)
         self.z = abs(odometry.pose.pose.orientation.z)
         self.w = abs(odometry.pose.pose.orientation.w)
 
+        self.detect_tip(odometry)
+        self.perform_robotic_tasks(self.time_threshold)
+        self.second_check(odometry)
+        self.check_for_hit_count(self.hit_count)
+
 
     def detect_tip(self, odometry):
-    # get rover orientation
-        print("x" + str(x))
-        print("y" + str(y))
-        print("z" + str(z))
-        print("w" + str(w))
+        # printing rover orientation
+        rospy.loginfo("self.x" + str(self.x))
+        rospy.loginfo("self.y" + str(self.y))
+        rospy.loginfo("self.z" + str(self.z))
+        rospy.loginfo("self.w" + str(self.w))
 
-    # hit count set to different threshold values; if more than x values out of 10 point out to the rover tipping over; the rover is tipping over
+        # hit count set to different threshold values; if more than x values out of 10 point out to the rover tipping over; the rover is tipping over
 
-        if w >= self.threshold:
-            self.hit_count +=1
-        elif x >= self.threshold:
-            self.hit_count +=1
-        elif y >= self.threshold:
-            self.hit_count +=1
+        if self.w >= self.threshold:
+            self.hit_count += 1
+        elif self.x >= self.threshold:
+            self.hit_count += 1
+        elif self.y >= self.threshold:
+            self.hit_count += 1
 
-    # checking velocity of rover
-    # check magnitude of angular velocity for each axis
+        # checking velocity of rover
+        # check magnitude of angular velocity for each axis
 
-    # create the separate angular velocity variables
+        # create the separate angular velocity variables
         angular_velocity_x = odometry.twist.twist.angular.x
         angular_velocity_y = odometry.twist.twist.angular.y
 
@@ -84,16 +90,16 @@ def detect_tip(self, odometry):
         angular_velocity_x_magnitude = abs[angular_velocity_x]
         angular_velocity_y_magnitude = abs[angular_velocity_y]
 
-        #check angular velocity magnitudes of each axis to see if it's above the threshold
+        # check angular velocity magnitudes of each axis to see if it's above the threshold
 
-    # Tipping over (pitch)
+        # Tipping over (pitch)
         if angular_velocity_x_magnitude > self.angular_threshold:
             self.hit_count += 1
-    # Rolling over (roll)
+        # Rolling over (roll)
         elif angular_velocity_y_magnitude > self.angular_threshold:
             self.hit_count += 1
 
-    #crates the time difference
+    # crates the time difference
     def perform_robotic_tasks(self, time_threshold):
         # Simulate  robotic tasks
         rospy.loginfo("Performing robotic tasks...")
@@ -101,9 +107,11 @@ def perform_robotic_tasks(self, time_threshold):
 
     # checking acceleration of rover
     def second_check(self, odometry):
-        linear_acceleration_norm = np.linalg.norm( np.array([odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z]))   
-        
-        rospy.init_node('elapsed_time_example_node', anonymous=True)
+        linear_acceleration_norm = np.linalg.norm(
+            np.array([odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z])
+        )
+
+        rospy.init_node("elapsed_time_example_node", anonymous=True)
         start_time = rospy.Time.now()
         self.perform_robotic_tasks(self.time_threshold)
         current_time = rospy.Time.now()
@@ -116,19 +124,19 @@ def second_check(self, odometry):
         linear_acceleration_y = abs(linear_acceleration_norm.y)
 
         if linear_acceleration_x == 0 and elapsed_seconds > self.time_threshold:
-            if w >= self.threshold:
-                hit_count +=1
-            elif x >= self.threshold:
-                hit_count +=1
-            elif y >= self.threshold:
-                hit_count +=1
-        elif linear_acceleration_y == 0 and elapsed_seconds  > self.time_threshold:
-            if w >= self.threshold:
-                hit_count +=1
-            elif x >= self.threshold:
-                hit_count +=1
-            elif y >= self.threshold:
-                hit_count +=1
+            if self.w >= self.threshold:
+                hit_count += 1
+            elif self.x >= self.threshold:
+                hit_count += 1
+            elif self.y >= self.threshold:
+                hit_count += 1
+        elif linear_acceleration_y == 0 and elapsed_seconds > self.time_threshold:
+            if self.w >= self.threshold:
+                hit_count += 1
+            elif self.x >= self.threshold:
+                hit_count += 1
+            elif self.y >= self.threshold:
+                hit_count += 1
 
     # seeing if hit_count is too big
     def check_for_hit_count(self, hit_count):
@@ -139,13 +147,17 @@ def check_for_hit_count(self, hit_count):
 def main():
     rospy.loginfo("===== tip detection starting =====")
     rospy.init_node("tip_detection")
-    tip_detection()
-    rospy.spin() #idk what this is im copying from failure_identification
+    TipDetection()
+    # tip_detection.detect_tip()
+    # tip_detection.perform_robotic_tasks()
+    # tip_detection.second_check()
+    # tip_detection.check_for_hit_count()
+
+    # rospy.spin()  # idk what this is im copying from failure_identification
+
 
-    
 if __name__ == "__main__":
     main()
-    
 
     """
     check if rover is tipped over over a certain period of time
@@ -176,4 +188,4 @@ def main():
 
     when talking ab multiplying quartonions, it's just adding them
     one 90 deg rotate + one 90 deg rotate = 180 deg rotate
-    """
\ No newline at end of file
+    """

From 650601fc3a6636b217b2dea485d6298b46fa4487 Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Thu, 18 Jan 2024 19:53:22 -0500
Subject: [PATCH 09/24] thresholds aren't working

---
 src/localization/tip_detection.py | 65 +++++++++++++------------------
 1 file changed, 28 insertions(+), 37 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index 740da8c57..8f9e74aea 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -24,21 +24,21 @@
 
 class TipDetection:
     hit_count: int
-    threshold: int
-    angular_threshold: int
-    linear_threshold: int
-    time_threshold: int
+    orientation_threshold: float
+    velocity_threshold_x: float
+    velocity_threshold_y: float
+    time_threshold: float
     hit_count_threshold: int
 
     def __init__(self):
         rospy.Subscriber("imu/imu_only", Imu, self.imu_callback)
         # read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
         self.hit_count = 0
-        self.threshold = 0
-        self.angular_threshold = 0
-        self.linear_threshold = 0
-        self.time_threshold = 0
-        self.hit_count_threshold = 0
+        self.orientation_threshold = 0.5
+        self.velocity_threshold_x = .025
+        self.velocity_threshold_y = .01
+        self.time_threshold = 1
+        self.hit_count_threshold = 5
         odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
         odometry_sub.registerCallback(self.odometry_callback)
         self.x = 0
@@ -62,7 +62,7 @@ def odometry_callback(self, odometry):
         self.second_check(odometry)
         self.check_for_hit_count(self.hit_count)
 
-
+    # checking the rover's velocity
     def detect_tip(self, odometry):
         # printing rover orientation
         rospy.loginfo("self.x" + str(self.x))
@@ -72,11 +72,11 @@ def detect_tip(self, odometry):
 
         # hit count set to different threshold values; if more than x values out of 10 point out to the rover tipping over; the rover is tipping over
 
-        if self.w >= self.threshold:
+        if self.w >= self.orientation_threshold:
             self.hit_count += 1
-        elif self.x >= self.threshold:
+        elif self.x >= self.orientation_threshold:
             self.hit_count += 1
-        elif self.y >= self.threshold:
+        elif self.y >= self.orientation_threshold:
             self.hit_count += 1
 
         # checking velocity of rover
@@ -87,31 +87,26 @@ def detect_tip(self, odometry):
         angular_velocity_y = odometry.twist.twist.angular.y
 
         # create the magnitude variables
-        angular_velocity_x_magnitude = abs[angular_velocity_x]
-        angular_velocity_y_magnitude = abs[angular_velocity_y]
+        angular_velocity_x_magnitude = abs(angular_velocity_x)
+        angular_velocity_y_magnitude = abs(angular_velocity_y)
 
         # check angular velocity magnitudes of each axis to see if it's above the threshold
 
         # Tipping over (pitch)
-        if angular_velocity_x_magnitude > self.angular_threshold:
+        if angular_velocity_x_magnitude > self.velocity_threshold_x:
             self.hit_count += 1
         # Rolling over (roll)
-        elif angular_velocity_y_magnitude > self.angular_threshold:
+        elif angular_velocity_y_magnitude > self.velocity_threshold_y:
             self.hit_count += 1
 
     # crates the time difference
     def perform_robotic_tasks(self, time_threshold):
-        # Simulate  robotic tasks
-        rospy.loginfo("Performing robotic tasks...")
+        # Simulate robotic tasks
+        rospy.loginfo("performing tasks...")
         rospy.sleep(time_threshold)  # Simulate a x second delay
 
     # checking acceleration of rover
-    def second_check(self, odometry):
-        linear_acceleration_norm = np.linalg.norm(
-            np.array([odometry.twist.twist.linear.x, odometry.twist.twist.linear.y, odometry.twist.twist.linear.z])
-        )
-
-        rospy.init_node("elapsed_time_example_node", anonymous=True)
+    def second_check(self, odometry):        
         start_time = rospy.Time.now()
         self.perform_robotic_tasks(self.time_threshold)
         current_time = rospy.Time.now()
@@ -120,22 +115,22 @@ def second_check(self, odometry):
         # Extract the elapsed time in seconds
         elapsed_seconds = elapsed_time.to_sec()
 
-        linear_acceleration_x = abs(linear_acceleration_norm.x)
-        linear_acceleration_y = abs(linear_acceleration_norm.y)
+        linear_acceleration_x = abs(odometry.twist.twist.linear.x)
+        linear_acceleration_y = abs(odometry.twist.twist.linear.y)
 
         if linear_acceleration_x == 0 and elapsed_seconds > self.time_threshold:
-            if self.w >= self.threshold:
+            if self.w >= self.orientation_threshold:
                 hit_count += 1
-            elif self.x >= self.threshold:
+            elif self.x >= self.orientation_threshold:
                 hit_count += 1
-            elif self.y >= self.threshold:
+            elif self.y >= self.orientation_threshold:
                 hit_count += 1
         elif linear_acceleration_y == 0 and elapsed_seconds > self.time_threshold:
-            if self.w >= self.threshold:
+            if self.w >= self.orientation_threshold:
                 hit_count += 1
-            elif self.x >= self.threshold:
+            elif self.x >= self.orientation_threshold:
                 hit_count += 1
-            elif self.y >= self.threshold:
+            elif self.y >= self.orientation_threshold:
                 hit_count += 1
 
     # seeing if hit_count is too big
@@ -148,10 +143,6 @@ def main():
     rospy.loginfo("===== tip detection starting =====")
     rospy.init_node("tip_detection")
     TipDetection()
-    # tip_detection.detect_tip()
-    # tip_detection.perform_robotic_tasks()
-    # tip_detection.second_check()
-    # tip_detection.check_for_hit_count()
 
     # rospy.spin()  # idk what this is im copying from failure_identification
 

From af8da695a12e9849fdf3e165b713b9c7538ba452 Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Sun, 21 Jan 2024 13:55:04 -0500
Subject: [PATCH 10/24] we love when there's output

---
 src/localization/tip_detection.py | 47 ++++++++++++++++---------------
 1 file changed, 24 insertions(+), 23 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index 8f9e74aea..823c90c78 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -17,10 +17,6 @@
 from nav_msgs.msg import Odometry
 from util.SE3 import SE3
 
-# detect what the pitch is like in the function
-# if the pitch is too high, make it a fail state
-# do fail states later?
-
 
 class TipDetection:
     hit_count: int
@@ -35,22 +31,26 @@ def __init__(self):
         # read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
         self.hit_count = 0
         self.orientation_threshold = 0.5
+        # these velocity thresholds mean the rover's always gonna be "tipping" if it's moving, right ? -audrey
+        # so, need to change to something else? why are we testing velocity?
         self.velocity_threshold_x = .025
         self.velocity_threshold_y = .01
         self.time_threshold = 1
         self.hit_count_threshold = 5
         odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
         odometry_sub.registerCallback(self.odometry_callback)
-        self.x = 0
-        self.y = 0
-        self.z = 0
-        self.w = 0
+        # self.x = 0
+        # self.y = 0
+        # self.z = 0
+        # self.w = 0
+
 
     def imu_callback(self, msg: Imu):
         pass
-        # self.pose = SE3.from_pos_quat(self.pose.position, np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]))
-        # self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
+        
 
+    # every time data is gotten from odometry, odometry_callback() is called from init()
+    # calling the rest of the functions like detect_tip() in odometry_callback()
     def odometry_callback(self, odometry):
         self.x = abs(odometry.pose.pose.orientation.x)
         self.y = abs(odometry.pose.pose.orientation.y)
@@ -62,6 +62,7 @@ def odometry_callback(self, odometry):
         self.second_check(odometry)
         self.check_for_hit_count(self.hit_count)
 
+
     # checking the rover's velocity
     def detect_tip(self, odometry):
         # printing rover orientation
@@ -70,8 +71,7 @@ def detect_tip(self, odometry):
         rospy.loginfo("self.z" + str(self.z))
         rospy.loginfo("self.w" + str(self.w))
 
-        # hit count set to different threshold values; if more than x values out of 10 point out to the rover tipping over; the rover is tipping over
-
+        # checking if orientation exceeds orientation_threshold, then increment hit_count
         if self.w >= self.orientation_threshold:
             self.hit_count += 1
         elif self.x >= self.orientation_threshold:
@@ -80,32 +80,30 @@ def detect_tip(self, odometry):
             self.hit_count += 1
 
         # checking velocity of rover
-        # check magnitude of angular velocity for each axis
 
         # create the separate angular velocity variables
-        angular_velocity_x = odometry.twist.twist.angular.x
-        angular_velocity_y = odometry.twist.twist.angular.y
-
-        # create the magnitude variables
-        angular_velocity_x_magnitude = abs(angular_velocity_x)
-        angular_velocity_y_magnitude = abs(angular_velocity_y)
+        angular_velocity_x = abs(odometry.twist.twist.angular.x)
+        angular_velocity_y = abs(odometry.twist.twist.angular.y)
 
         # check angular velocity magnitudes of each axis to see if it's above the threshold
 
         # Tipping over (pitch)
-        if angular_velocity_x_magnitude > self.velocity_threshold_x:
+        if angular_velocity_x >= self.velocity_threshold_x:
             self.hit_count += 1
         # Rolling over (roll)
-        elif angular_velocity_y_magnitude > self.velocity_threshold_y:
+        elif angular_velocity_y >= self.velocity_threshold_y:
             self.hit_count += 1
 
+
     # crates the time difference
     def perform_robotic_tasks(self, time_threshold):
         # Simulate robotic tasks
         rospy.loginfo("performing tasks...")
         rospy.sleep(time_threshold)  # Simulate a x second delay
 
+
     # checking acceleration of rover
+    # why don't we have an acceleration threshold? why are we using orientation threshold? -audrey
     def second_check(self, odometry):        
         start_time = rospy.Time.now()
         self.perform_robotic_tasks(self.time_threshold)
@@ -118,6 +116,8 @@ def second_check(self, odometry):
         linear_acceleration_x = abs(odometry.twist.twist.linear.x)
         linear_acceleration_y = abs(odometry.twist.twist.linear.y)
 
+        # checking if the acceleration exceeds our threshold ?
+        # not sure why we're checking for 0, need clarification ^^ -audrey
         if linear_acceleration_x == 0 and elapsed_seconds > self.time_threshold:
             if self.w >= self.orientation_threshold:
                 hit_count += 1
@@ -133,10 +133,11 @@ def second_check(self, odometry):
             elif self.y >= self.orientation_threshold:
                 hit_count += 1
 
+
     # seeing if hit_count is too big
     def check_for_hit_count(self, hit_count):
         if hit_count > self.hit_count_threshold:
-            print("tipping")
+            rospy.loginfo("tipping")
 
 
 def main():
@@ -144,7 +145,7 @@ def main():
     rospy.init_node("tip_detection")
     TipDetection()
 
-    # rospy.spin()  # idk what this is im copying from failure_identification
+    rospy.spin() # taken from failure identification but idk what it does exactly
 
 
 if __name__ == "__main__":

From f1433e6d796bd696919016b3eea6105a21210dfc Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Thu, 25 Jan 2024 19:48:00 -0500
Subject: [PATCH 11/24] i <3 the new simulator, how to see when we decide if
 it's tipping?

---
 scripts/debug_course_publisher.py | 22 +++++++++++-----------
 src/localization/tip_detection.py | 29 ++++++++++++-----------------
 2 files changed, 23 insertions(+), 28 deletions(-)

diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py
index c14c7891c..42ad39e19 100755
--- a/scripts/debug_course_publisher.py
+++ b/scripts/debug_course_publisher.py
@@ -18,16 +18,16 @@
 
     publish_waypoints([convert_waypoint_to_gps(waypoint) for waypoint in [
         (
-            Waypoint(fiducial_id=0, tf_id="course0", type=WaypointType(val=WaypointType.NO_SEARCH)),
+            Waypoint(tag_id=0, type=WaypointType(val=WaypointType.NO_SEARCH)),
             # SE3(position=np.array([-844.37, 10351.56, 0])),
-            SE3(position=np.array([15, -20, 0])),
-        ),
-        (
-            Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)),
-            SE3(position=np.array([-2, -2, 0])),
-        ),
-        (
-            Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)),
-            SE3(position=np.array([11, -10, 0])),
-        )
+            SE3(position=np.array([-30, -15, 0])),
+         )#,
+        # (
+        #     Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)),
+        #     SE3(position=np.array([-2, -2, 0])),
+        # ),
+        # (
+        #     Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)),
+        #     SE3(position=np.array([11, -10, 0])),
+        # )
     ]])
diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index 823c90c78..a075a810d 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -21,8 +21,8 @@
 class TipDetection:
     hit_count: int
     orientation_threshold: float
-    velocity_threshold_x: float
-    velocity_threshold_y: float
+    angular_velocity_threshold_x: float
+    angular_velocity_threshold_y: float
     time_threshold: float
     hit_count_threshold: int
 
@@ -31,18 +31,12 @@ def __init__(self):
         # read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
         self.hit_count = 0
         self.orientation_threshold = 0.5
-        # these velocity thresholds mean the rover's always gonna be "tipping" if it's moving, right ? -audrey
-        # so, need to change to something else? why are we testing velocity?
-        self.velocity_threshold_x = .025
-        self.velocity_threshold_y = .01
+        self.angular_velocity_threshold_x = .25 #pitch
+        self.angular_velocity_threshold_y = .25 #roll
         self.time_threshold = 1
         self.hit_count_threshold = 5
         odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
         odometry_sub.registerCallback(self.odometry_callback)
-        # self.x = 0
-        # self.y = 0
-        # self.z = 0
-        # self.w = 0
 
 
     def imu_callback(self, msg: Imu):
@@ -74,12 +68,13 @@ def detect_tip(self, odometry):
         # checking if orientation exceeds orientation_threshold, then increment hit_count
         if self.w >= self.orientation_threshold:
             self.hit_count += 1
-        elif self.x >= self.orientation_threshold:
-            self.hit_count += 1
-        elif self.y >= self.orientation_threshold:
-            self.hit_count += 1
+        # elif self.x >= self.orientation_threshold:
+        #     self.hit_count += 1
+        # elif self.y >= self.orientation_threshold:
+        #     self.hit_count += 1
+        # commented this out- don't we only need w? - aud
 
-        # checking velocity of rover
+        # checking angular velocity of rover
 
         # create the separate angular velocity variables
         angular_velocity_x = abs(odometry.twist.twist.angular.x)
@@ -88,10 +83,10 @@ def detect_tip(self, odometry):
         # check angular velocity magnitudes of each axis to see if it's above the threshold
 
         # Tipping over (pitch)
-        if angular_velocity_x >= self.velocity_threshold_x:
+        if angular_velocity_x >= self.angular_velocity_threshold_x:
             self.hit_count += 1
         # Rolling over (roll)
-        elif angular_velocity_y >= self.velocity_threshold_y:
+        elif angular_velocity_y >= self.angular_velocity_threshold_y:
             self.hit_count += 1
 
 

From d73c0025e996ece4cfc7d95a9c706fc7469fddc1 Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Tue, 30 Jan 2024 19:54:23 -0500
Subject: [PATCH 12/24] how/when to reset hit_count?

---
 launch/localization.launch        |  1 +
 src/localization/tip_detection.py | 37 ++++++++++++++++++++++---------
 2 files changed, 28 insertions(+), 10 deletions(-)

diff --git a/launch/localization.launch b/launch/localization.launch
index 18dac17d1..8f68cd3af 100644
--- a/launch/localization.launch
+++ b/launch/localization.launch
@@ -19,4 +19,5 @@
 
     <!-- Linearize GPS from geodetic to ENU cartesian coords -->
     <node name="gps_linearization" pkg="mrover" type="gps_linearization.py" output="screen" />
+    <node name="tip_detection" pkg="mrover" type="tip_detection.py" output="screen" />
 </launch>
\ No newline at end of file
diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index a075a810d..dc78a3718 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -25,6 +25,8 @@ class TipDetection:
     angular_velocity_threshold_y: float
     time_threshold: float
     hit_count_threshold: int
+    reset_hit_count_threshold: int
+    time_counter: int
 
     def __init__(self):
         rospy.Subscriber("imu/imu_only", Imu, self.imu_callback)
@@ -35,8 +37,12 @@ def __init__(self):
         self.angular_velocity_threshold_y = .25 #roll
         self.time_threshold = 1
         self.hit_count_threshold = 5
+        self.reset_hit_count_threshold = 10
+        self.time_counter = 0
+
         odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
         odometry_sub.registerCallback(self.odometry_callback)
+        #print("init done")
 
 
     def imu_callback(self, msg: Imu):
@@ -46,6 +52,7 @@ def imu_callback(self, msg: Imu):
     # every time data is gotten from odometry, odometry_callback() is called from init()
     # calling the rest of the functions like detect_tip() in odometry_callback()
     def odometry_callback(self, odometry):
+
         self.x = abs(odometry.pose.pose.orientation.x)
         self.y = abs(odometry.pose.pose.orientation.y)
         self.z = abs(odometry.pose.pose.orientation.z)
@@ -55,6 +62,8 @@ def odometry_callback(self, odometry):
         self.perform_robotic_tasks(self.time_threshold)
         self.second_check(odometry)
         self.check_for_hit_count(self.hit_count)
+        self.reset_hit_count_time(self.time_threshold)
+        self.time_counter += 1
 
 
     # checking the rover's velocity
@@ -68,11 +77,6 @@ def detect_tip(self, odometry):
         # checking if orientation exceeds orientation_threshold, then increment hit_count
         if self.w >= self.orientation_threshold:
             self.hit_count += 1
-        # elif self.x >= self.orientation_threshold:
-        #     self.hit_count += 1
-        # elif self.y >= self.orientation_threshold:
-        #     self.hit_count += 1
-        # commented this out- don't we only need w? - aud
 
         # checking angular velocity of rover
 
@@ -86,7 +90,7 @@ def detect_tip(self, odometry):
         if angular_velocity_x >= self.angular_velocity_threshold_x:
             self.hit_count += 1
         # Rolling over (roll)
-        elif angular_velocity_y >= self.angular_velocity_threshold_y:
+        if angular_velocity_y >= self.angular_velocity_threshold_y:
             self.hit_count += 1
 
 
@@ -99,7 +103,8 @@ def perform_robotic_tasks(self, time_threshold):
 
     # checking acceleration of rover
     # why don't we have an acceleration threshold? why are we using orientation threshold? -audrey
-    def second_check(self, odometry):        
+    def second_check(self, odometry):    
+        pass    
         start_time = rospy.Time.now()
         self.perform_robotic_tasks(self.time_threshold)
         current_time = rospy.Time.now()
@@ -133,6 +138,19 @@ def second_check(self, odometry):
     def check_for_hit_count(self, hit_count):
         if hit_count > self.hit_count_threshold:
             rospy.loginfo("tipping")
+            rospy.loginfo(hit_count)
+        else:
+            rospy.loginfo('not tipping')
+            rospy.loginfo(hit_count)
+
+
+    def reset_hit_count_time(self, reset_hit_count_threshold):
+        if self.time_counter > self.reset_hit_count_threshold:
+            rospy.loginfo("resetting hit count...")
+            rospy.sleep(reset_hit_count_threshold)  # Simulate a x second delay
+            self.hit_count = 0
+            self.time_counter = 0
+
 
 
 def main():
@@ -143,6 +161,8 @@ def main():
     rospy.spin() # taken from failure identification but idk what it does exactly
 
 
+
+
 if __name__ == "__main__":
     main()
 
@@ -151,12 +171,9 @@ def main():
     if there are a lot of fluctuations over an arbitrary time period,
     then the rover is tipped
 
-
     ex. if there are 50 over-tipped measurements over 10 secs, then tipped
     made up numbers ^^
 
-
-
     ways we can check the cases for tipping:
     - measure magnitudes of angular velocity and see if there is one that is above a threshold
     - velocity is = or near 0 for a set amount of time

From 871065235f0839a6f052468f7a28b599d7117583 Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Sun, 4 Feb 2024 13:36:20 -0500
Subject: [PATCH 13/24] resetting time work

---
 src/localization/tip_detection.py | 32 +++++++++++++++----------------
 1 file changed, 15 insertions(+), 17 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index dc78a3718..ac86bb115 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -11,11 +11,11 @@
 from mrover.msg import MotorsStatus
 from nav_msgs.msg import Odometry
 from pandas import DataFrame
-from smach_msgs.msg import SmachContainerStatus
 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:
@@ -26,7 +26,7 @@ class TipDetection:
     time_threshold: float
     hit_count_threshold: int
     reset_hit_count_threshold: int
-    time_counter: int
+    time_counter: time
 
     def __init__(self):
         rospy.Subscriber("imu/imu_only", Imu, self.imu_callback)
@@ -38,11 +38,10 @@ def __init__(self):
         self.time_threshold = 1
         self.hit_count_threshold = 5
         self.reset_hit_count_threshold = 10
-        self.time_counter = 0
+        self.time_counter = time.time()
 
         odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
         odometry_sub.registerCallback(self.odometry_callback)
-        #print("init done")
 
 
     def imu_callback(self, msg: Imu):
@@ -62,8 +61,8 @@ def odometry_callback(self, odometry):
         self.perform_robotic_tasks(self.time_threshold)
         self.second_check(odometry)
         self.check_for_hit_count(self.hit_count)
-        self.reset_hit_count_time(self.time_threshold)
-        self.time_counter += 1
+        self.reset_hit_count_time(self.reset_hit_count_threshold, self.time_counter)
+
 
 
     # checking the rover's velocity
@@ -120,18 +119,18 @@ def second_check(self, odometry):
         # not sure why we're checking for 0, need clarification ^^ -audrey
         if linear_acceleration_x == 0 and elapsed_seconds > self.time_threshold:
             if self.w >= self.orientation_threshold:
-                hit_count += 1
+                self.hit_count += 1
             elif self.x >= self.orientation_threshold:
-                hit_count += 1
+                self.hit_count += 1
             elif self.y >= self.orientation_threshold:
-                hit_count += 1
+                self.hit_count += 1
         elif linear_acceleration_y == 0 and elapsed_seconds > self.time_threshold:
             if self.w >= self.orientation_threshold:
-                hit_count += 1
+                self.hit_count += 1
             elif self.x >= self.orientation_threshold:
-                hit_count += 1
+                self.hit_count += 1
             elif self.y >= self.orientation_threshold:
-                hit_count += 1
+                self.hit_count += 1
 
 
     # seeing if hit_count is too big
@@ -144,12 +143,12 @@ def check_for_hit_count(self, hit_count):
             rospy.loginfo(hit_count)
 
 
-    def reset_hit_count_time(self, reset_hit_count_threshold):
-        if self.time_counter > self.reset_hit_count_threshold:
+    def reset_hit_count_time(self, reset_hit_count_threshold, time_counter):
+        rospy.loginfo(self.time_counter)
+        if time.time() - self.time_counter > self.reset_hit_count_threshold:
             rospy.loginfo("resetting hit count...")
-            rospy.sleep(reset_hit_count_threshold)  # Simulate a x second delay
             self.hit_count = 0
-            self.time_counter = 0
+            self.time_counter = time.time()
 
 
 
@@ -162,7 +161,6 @@ def main():
 
 
 
-
 if __name__ == "__main__":
     main()
 

From 321d6dd6cf993642d83473ddfc898368b1fbd019 Mon Sep 17 00:00:00 2001
From: nehakankanala <nehakan@umich.edu>
Date: Tue, 6 Feb 2024 17:56:04 -0500
Subject: [PATCH 14/24] started adding getting pose from tf tree

---
 src/localization/tip_detection.py | 47 ++++++++++++++++++-------------
 1 file changed, 27 insertions(+), 20 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index ac86bb115..7b20aa35c 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -33,20 +33,35 @@ def __init__(self):
         # read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
         self.hit_count = 0
         self.orientation_threshold = 0.5
-        self.angular_velocity_threshold_x = .25 #pitch
-        self.angular_velocity_threshold_y = .25 #roll
+        self.angular_velocity_threshold_x = 0.25  # pitch
+        self.angular_velocity_threshold_y = 0.25  # roll
         self.time_threshold = 1
         self.hit_count_threshold = 5
         self.reset_hit_count_threshold = 10
         self.time_counter = time.time()
 
-        odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
-        odometry_sub.registerCallback(self.odometry_callback)
-
+        self.buffer = tf2_ros.Buffer()
+        self.world_frame = rospy.get_param("world_frame")
+        self.rover_frame = rospy.get_param("rover_frame")
+        print(self.world_frame)
+        print(self.rover_frame)
+        self.in_loop()
+
+        # odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
+        # odometry_sub.registerCallback(self.odometry_callback)
+
+    def in_loop(self):
+        rate = rospy.Rate(10.0)
+        while not rospy.is_shutdown():
+            try:
+                print(SE3.from_tf_tree(self.buffer, self.world_frame, self.rover_frame))
+            except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
+                print(e)
+                rate.sleep()
+        
 
     def imu_callback(self, msg: Imu):
         pass
-        
 
     # every time data is gotten from odometry, odometry_callback() is called from init()
     # calling the rest of the functions like detect_tip() in odometry_callback()
@@ -63,8 +78,6 @@ def odometry_callback(self, odometry):
         self.check_for_hit_count(self.hit_count)
         self.reset_hit_count_time(self.reset_hit_count_threshold, self.time_counter)
 
-
-
     # checking the rover's velocity
     def detect_tip(self, odometry):
         # printing rover orientation
@@ -92,18 +105,16 @@ def detect_tip(self, odometry):
         if angular_velocity_y >= self.angular_velocity_threshold_y:
             self.hit_count += 1
 
-
     # crates the time difference
     def perform_robotic_tasks(self, time_threshold):
         # Simulate robotic tasks
         rospy.loginfo("performing tasks...")
         rospy.sleep(time_threshold)  # Simulate a x second delay
 
-
     # checking acceleration of rover
     # why don't we have an acceleration threshold? why are we using orientation threshold? -audrey
-    def second_check(self, odometry):    
-        pass    
+    def second_check(self, odometry):
+        pass
         start_time = rospy.Time.now()
         self.perform_robotic_tasks(self.time_threshold)
         current_time = rospy.Time.now()
@@ -132,33 +143,29 @@ def second_check(self, odometry):
             elif self.y >= self.orientation_threshold:
                 self.hit_count += 1
 
-
     # seeing if hit_count is too big
     def check_for_hit_count(self, hit_count):
         if hit_count > self.hit_count_threshold:
             rospy.loginfo("tipping")
             rospy.loginfo(hit_count)
         else:
-            rospy.loginfo('not tipping')
+            rospy.loginfo("not tipping")
             rospy.loginfo(hit_count)
 
-
     def reset_hit_count_time(self, reset_hit_count_threshold, time_counter):
-        rospy.loginfo(self.time_counter)
+        print(f"time {time.time()- self.time_counter}")
         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()
-
-    rospy.spin() # taken from failure identification but idk what it does exactly
+    tip_detector = TipDetection()
 
+    # rospy.spin()  # taken from failure identification but idk what it does exactly
 
 
 if __name__ == "__main__":

From 401056e44675fac9edd21239d7787c850c74522d Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Tue, 6 Feb 2024 19:50:47 -0500
Subject: [PATCH 15/24] added to in_loop()

---
 src/localization/tip_detection.py | 25 ++++++++++++++++++++++---
 1 file changed, 22 insertions(+), 3 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index 7b20aa35c..f63465a60 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -21,6 +21,7 @@
 class TipDetection:
     hit_count: int
     orientation_threshold: float
+    z_orientation_threshold: float
     angular_velocity_threshold_x: float
     angular_velocity_threshold_y: float
     time_threshold: float
@@ -33,6 +34,7 @@ def __init__(self):
         # read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
         self.hit_count = 0
         self.orientation_threshold = 0.5
+        self.z_orientation_threshold = 0.5 # may need to change this
         self.angular_velocity_threshold_x = 0.25  # pitch
         self.angular_velocity_threshold_y = 0.25  # roll
         self.time_threshold = 1
@@ -54,6 +56,23 @@ def in_loop(self):
         rate = rospy.Rate(10.0)
         while not rospy.is_shutdown():
             try:
+                # extract the  matrix by [0,0,1] (z vector). this could be completely wrong
+                self.old = SE3.from_tf_tree(self.buffer, self.world_frame, self.rover_frame)
+                self.transform = []
+                for i in range(3):
+                    for j in range(3):
+                        self.transform.append([[self.old[i][j] for b in range(j, j + 3)]
+                                          for a in range(i, i + 3)
+                                          ]
+                        )
+
+                # multiply this transform by the z vector [0, 0, 1]
+                self.transform = np.array([0, 0, 1]) * self.transform  # ?
+
+                # compare this new transform with our threshold to see if it's tipping, if so increment hit_count
+                if self.transform >= self.z_orientation_threshold:
+                    self.hit_count += 1
+                
                 print(SE3.from_tf_tree(self.buffer, self.world_frame, self.rover_frame))
             except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
                 print(e)
@@ -126,8 +145,8 @@ def second_check(self, odometry):
         linear_acceleration_x = abs(odometry.twist.twist.linear.x)
         linear_acceleration_y = abs(odometry.twist.twist.linear.y)
 
-        # checking if the acceleration exceeds our threshold ?
-        # not sure why we're checking for 0, need clarification ^^ -audrey
+        # checking if the acceleration exceeds our threshold
+        # why are we checking if it's 0 -aud
         if linear_acceleration_x == 0 and elapsed_seconds > self.time_threshold:
             if self.w >= self.orientation_threshold:
                 self.hit_count += 1
@@ -165,7 +184,7 @@ def main():
     rospy.init_node("tip_detection")
     tip_detector = TipDetection()
 
-    # rospy.spin()  # taken from failure identification but idk what it does exactly
+    # rospy.spin()  
 
 
 if __name__ == "__main__":

From 33dbc10a098270a4a7d82ee2b2973a0c86ab3429 Mon Sep 17 00:00:00 2001
From: nehakankanala <nehakan@umich.edu>
Date: Sat, 10 Feb 2024 12:00:58 -0500
Subject: [PATCH 16/24] fixed tf tree issue

---
 src/localization/tip_detection.py | 27 ++++++++++-----------------
 1 file changed, 10 insertions(+), 17 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index f63465a60..789314640 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -34,7 +34,7 @@ def __init__(self):
         # read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
         self.hit_count = 0
         self.orientation_threshold = 0.5
-        self.z_orientation_threshold = 0.5 # may need to change this
+        self.z_orientation_threshold = 0.5  # may need to change this
         self.angular_velocity_threshold_x = 0.25  # pitch
         self.angular_velocity_threshold_y = 0.25  # roll
         self.time_threshold = 1
@@ -43,6 +43,7 @@ def __init__(self):
         self.time_counter = time.time()
 
         self.buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.buffer)
         self.world_frame = rospy.get_param("world_frame")
         self.rover_frame = rospy.get_param("rover_frame")
         print(self.world_frame)
@@ -57,27 +58,19 @@ def in_loop(self):
         while not rospy.is_shutdown():
             try:
                 # extract the  matrix by [0,0,1] (z vector). this could be completely wrong
-                self.old = SE3.from_tf_tree(self.buffer, self.world_frame, self.rover_frame)
-                self.transform = []
-                for i in range(3):
-                    for j in range(3):
-                        self.transform.append([[self.old[i][j] for b in range(j, j + 3)]
-                                          for a in range(i, i + 3)
-                                          ]
-                        )
-
+                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]
-                self.transform = np.array([0, 0, 1]) * self.transform  # ?
+                self.transform = np.dot(np.array([0, 0, 1]), self.old)  # ?
+                print(self.transform[2])
 
                 # compare this new transform with our threshold to see if it's tipping, if so increment hit_count
-                if self.transform >= self.z_orientation_threshold:
-                    self.hit_count += 1
-                
-                print(SE3.from_tf_tree(self.buffer, self.world_frame, self.rover_frame))
+                # if self.transform >= self.z_orientation_threshold:
+                #     self.hit_count += 1
+
+                # print(SE3.from_tf_tree(self.buffer, self.world_frame, self.rover_frame))
             except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
                 print(e)
                 rate.sleep()
-        
 
     def imu_callback(self, msg: Imu):
         pass
@@ -184,7 +177,7 @@ def main():
     rospy.init_node("tip_detection")
     tip_detector = TipDetection()
 
-    # rospy.spin()  
+    rospy.spin()
 
 
 if __name__ == "__main__":

From 78d41f5fbe1149d996e4bd9cbde01ce9ca68b641 Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Sun, 11 Feb 2024 13:45:49 -0500
Subject: [PATCH 17/24] just have to fix thresholds

---
 src/localization/tip_detection.py | 21 ++++++++++++++-------
 1 file changed, 14 insertions(+), 7 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index 789314640..e8dbdcb1c 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -28,19 +28,22 @@ class TipDetection:
     hit_count_threshold: int
     reset_hit_count_threshold: int
     time_counter: time
+    current_time: time
 
     def __init__(self):
         rospy.Subscriber("imu/imu_only", Imu, self.imu_callback)
         # read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
         self.hit_count = 0
+        self.z_orientation_threshold = 0.7
+        self.hit_count_threshold = 5
+        self.reset_hit_count_threshold = 3
+        self.time_counter = time.time()
+        self.current_time = time.time()
+
         self.orientation_threshold = 0.5
-        self.z_orientation_threshold = 0.5  # may need to change this
         self.angular_velocity_threshold_x = 0.25  # pitch
         self.angular_velocity_threshold_y = 0.25  # roll
         self.time_threshold = 1
-        self.hit_count_threshold = 5
-        self.reset_hit_count_threshold = 10
-        self.time_counter = time.time()
 
         self.buffer = tf2_ros.Buffer()
         self.tf_listener = tf2_ros.TransformListener(self.buffer)
@@ -60,18 +63,22 @@ def in_loop(self):
                 # extract the  matrix by [0,0,1] (z vector). this could be completely wrong
                 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]
-                self.transform = np.dot(np.array([0, 0, 1]), self.old)  # ?
+                self.transform = np.dot(np.array([0, 0, 1]), self.old)
                 print(self.transform[2])
 
                 # compare this new transform with our threshold to see if it's tipping, if so increment hit_count
-                # if self.transform >= self.z_orientation_threshold:
-                #     self.hit_count += 1
+                if self.transform[2] <= self.z_orientation_threshold:
+                    self.hit_count += 1
+                    self.check_for_hit_count(self.hit_count)
 
                 # print(SE3.from_tf_tree(self.buffer, self.world_frame, self.rover_frame))
             except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
                 print(e)
                 rate.sleep()
 
+            self.current_time = time.time()
+            self.reset_hit_count_time(self.reset_hit_count_threshold, self.time_counter)
+
     def imu_callback(self, msg: Imu):
         pass
 

From 4de614fe603e3579c15eff1a8fc93e154eda1d34 Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Tue, 13 Feb 2024 18:27:59 -0500
Subject: [PATCH 18/24] small

---
 src/localization/tip_detection.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index e8dbdcb1c..15d27491e 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -34,8 +34,8 @@ def __init__(self):
         rospy.Subscriber("imu/imu_only", Imu, self.imu_callback)
         # read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
         self.hit_count = 0
-        self.z_orientation_threshold = 0.7
-        self.hit_count_threshold = 5
+        self.z_orientation_threshold = 0.65
+        self.hit_count_threshold = 100
         self.reset_hit_count_threshold = 3
         self.time_counter = time.time()
         self.current_time = time.time()
@@ -167,9 +167,9 @@ def check_for_hit_count(self, hit_count):
         if hit_count > self.hit_count_threshold:
             rospy.loginfo("tipping")
             rospy.loginfo(hit_count)
-        else:
-            rospy.loginfo("not tipping")
-            rospy.loginfo(hit_count)
+        # else:
+        #     rospy.loginfo("not tipping")
+        #     rospy.loginfo(hit_count)
 
     def reset_hit_count_time(self, reset_hit_count_threshold, time_counter):
         print(f"time {time.time()- self.time_counter}")

From f09f9146273307989a52bdfb7f7be68577119fc6 Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Tue, 13 Feb 2024 19:11:38 -0500
Subject: [PATCH 19/24] publishing

---
 src/localization/tip_detection.py | 150 +++---------------------------
 1 file changed, 13 insertions(+), 137 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index 15d27491e..9bd6bf1bd 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -20,58 +20,45 @@
 
 class TipDetection:
     hit_count: int
-    orientation_threshold: float
     z_orientation_threshold: float
-    angular_velocity_threshold_x: float
-    angular_velocity_threshold_y: float
-    time_threshold: float
     hit_count_threshold: int
     reset_hit_count_threshold: int
     time_counter: time
     current_time: time
+    tip_publisher: rospy.Publisher
 
     def __init__(self):
         rospy.Subscriber("imu/imu_only", Imu, self.imu_callback)
-        # read the orientation data from the given Imu message, store that value in `self.pose`, then publish that pose to the TF tree.
+        self.tip_publisher = rospy.Publisher("tipping", int, queue_size=1)
+
         self.hit_count = 0
-        self.z_orientation_threshold = 0.65
-        self.hit_count_threshold = 100
+        self.z_orientation_threshold = 0.8
+        self.hit_count_threshold = 10
         self.reset_hit_count_threshold = 3
         self.time_counter = time.time()
         self.current_time = time.time()
 
-        self.orientation_threshold = 0.5
-        self.angular_velocity_threshold_x = 0.25  # pitch
-        self.angular_velocity_threshold_y = 0.25  # roll
-        self.time_threshold = 1
-
         self.buffer = tf2_ros.Buffer()
         self.tf_listener = tf2_ros.TransformListener(self.buffer)
         self.world_frame = rospy.get_param("world_frame")
         self.rover_frame = rospy.get_param("rover_frame")
-        print(self.world_frame)
-        print(self.rover_frame)
         self.in_loop()
 
-        # odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
-        # odometry_sub.registerCallback(self.odometry_callback)
-
     def in_loop(self):
         rate = rospy.Rate(10.0)
         while not rospy.is_shutdown():
             try:
-                # extract the  matrix by [0,0,1] (z vector). this could be completely wrong
+                # extract the  matrix by [0,0,1] (z vector)
                 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]
                 self.transform = np.dot(np.array([0, 0, 1]), self.old)
-                print(self.transform[2])
 
                 # compare this new transform with our threshold to see if it's tipping, if so increment hit_count
                 if self.transform[2] <= self.z_orientation_threshold:
                     self.hit_count += 1
                     self.check_for_hit_count(self.hit_count)
 
-                # print(SE3.from_tf_tree(self.buffer, self.world_frame, self.rover_frame))
             except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
                 print(e)
                 rate.sleep()
@@ -79,98 +66,15 @@ def in_loop(self):
             self.current_time = time.time()
             self.reset_hit_count_time(self.reset_hit_count_threshold, self.time_counter)
 
-    def imu_callback(self, msg: Imu):
-        pass
-
-    # every time data is gotten from odometry, odometry_callback() is called from init()
-    # calling the rest of the functions like detect_tip() in odometry_callback()
-    def odometry_callback(self, odometry):
-
-        self.x = abs(odometry.pose.pose.orientation.x)
-        self.y = abs(odometry.pose.pose.orientation.y)
-        self.z = abs(odometry.pose.pose.orientation.z)
-        self.w = abs(odometry.pose.pose.orientation.w)
-
-        self.detect_tip(odometry)
-        self.perform_robotic_tasks(self.time_threshold)
-        self.second_check(odometry)
-        self.check_for_hit_count(self.hit_count)
-        self.reset_hit_count_time(self.reset_hit_count_threshold, self.time_counter)
-
-    # checking the rover's velocity
-    def detect_tip(self, odometry):
-        # printing rover orientation
-        rospy.loginfo("self.x" + str(self.x))
-        rospy.loginfo("self.y" + str(self.y))
-        rospy.loginfo("self.z" + str(self.z))
-        rospy.loginfo("self.w" + str(self.w))
-
-        # checking if orientation exceeds orientation_threshold, then increment hit_count
-        if self.w >= self.orientation_threshold:
-            self.hit_count += 1
-
-        # checking angular velocity of rover
-
-        # create the separate angular velocity variables
-        angular_velocity_x = abs(odometry.twist.twist.angular.x)
-        angular_velocity_y = abs(odometry.twist.twist.angular.y)
-
-        # check angular velocity magnitudes of each axis to see if it's above the threshold
-
-        # Tipping over (pitch)
-        if angular_velocity_x >= self.angular_velocity_threshold_x:
-            self.hit_count += 1
-        # Rolling over (roll)
-        if angular_velocity_y >= self.angular_velocity_threshold_y:
-            self.hit_count += 1
-
-    # crates the time difference
-    def perform_robotic_tasks(self, time_threshold):
-        # Simulate robotic tasks
-        rospy.loginfo("performing tasks...")
-        rospy.sleep(time_threshold)  # Simulate a x second delay
-
-    # checking acceleration of rover
-    # why don't we have an acceleration threshold? why are we using orientation threshold? -audrey
-    def second_check(self, odometry):
-        pass
-        start_time = rospy.Time.now()
-        self.perform_robotic_tasks(self.time_threshold)
-        current_time = rospy.Time.now()
-        # Calculate the time difference
-        elapsed_time = current_time - start_time
-        # Extract the elapsed time in seconds
-        elapsed_seconds = elapsed_time.to_sec()
-
-        linear_acceleration_x = abs(odometry.twist.twist.linear.x)
-        linear_acceleration_y = abs(odometry.twist.twist.linear.y)
-
-        # checking if the acceleration exceeds our threshold
-        # why are we checking if it's 0 -aud
-        if linear_acceleration_x == 0 and elapsed_seconds > self.time_threshold:
-            if self.w >= self.orientation_threshold:
-                self.hit_count += 1
-            elif self.x >= self.orientation_threshold:
-                self.hit_count += 1
-            elif self.y >= self.orientation_threshold:
-                self.hit_count += 1
-        elif linear_acceleration_y == 0 and elapsed_seconds > self.time_threshold:
-            if self.w >= self.orientation_threshold:
-                self.hit_count += 1
-            elif self.x >= self.orientation_threshold:
-                self.hit_count += 1
-            elif self.y >= self.orientation_threshold:
-                self.hit_count += 1
-
     # seeing if hit_count is too big
     def check_for_hit_count(self, hit_count):
         if hit_count > self.hit_count_threshold:
             rospy.loginfo("tipping")
             rospy.loginfo(hit_count)
-        # else:
-        #     rospy.loginfo("not tipping")
-        #     rospy.loginfo(hit_count)
+            # publishing into tip_publisher that rover is tipping, true(1)
+            self.tip_publisher.publish(1)
 
+    # resetting 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 time.time() - self.time_counter > self.reset_hit_count_threshold:
@@ -178,42 +82,14 @@ def reset_hit_count_time(self, reset_hit_count_threshold, time_counter):
             self.hit_count = 0
             self.time_counter = time.time()
 
-
 def main():
-    rospy.loginfo("===== tip detection starting =====")
+    # rospy.loginfo("===== tip detection starting =====")
     rospy.init_node("tip_detection")
-    tip_detector = TipDetection()
+    TipDetection()
 
     rospy.spin()
 
 
 if __name__ == "__main__":
     main()
-
-    """
-    check if rover is tipped over over a certain period of time
-    if there are a lot of fluctuations over an arbitrary time period,
-    then the rover is tipped
-
-    ex. if there are 50 over-tipped measurements over 10 secs, then tipped
-    made up numbers ^^
-
-    ways we can check the cases for tipping:
-    - measure magnitudes of angular velocity and see if there is one that is above a threshold
-    - velocity is = or near 0 for a set amount of time
-    - check orientation
-
-    quartonions:
-    basically weird way to store the way the rotations are rotating
-    qx = ax * sin(angle/2)
-    qy = ay * sin(angle/2)
-    qz = az * sin(angle/2)
-    qw = cos(angle/2)
-
-    ax, ay, and az are how much it is angled off of each axis
-    phone rotating on table: 0, 0, 1
-    w is how much it's being rotated based on x,y,z
-
-    when talking ab multiplying quartonions, it's just adding them
-    one 90 deg rotate + one 90 deg rotate = 180 deg rotate
-    """
+    
\ No newline at end of file

From 5dcd264c5a8e4b9fc4922b191b4e387f1a10fc84 Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Tue, 13 Feb 2024 19:38:52 -0500
Subject: [PATCH 20/24] we love publishing

---
 src/localization/tip_detection.py | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index 9bd6bf1bd..3432eb529 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -28,8 +28,7 @@ class TipDetection:
     tip_publisher: rospy.Publisher
 
     def __init__(self):
-        rospy.Subscriber("imu/imu_only", Imu, self.imu_callback)
-        self.tip_publisher = rospy.Publisher("tipping", int, queue_size=1)
+        self.tip_publisher = rospy.Publisher("tipping", Bool, queue_size=1)
 
         self.hit_count = 0
         self.z_orientation_threshold = 0.8
@@ -55,6 +54,7 @@ def in_loop(self):
                 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:
                     self.hit_count += 1
                     self.check_for_hit_count(self.hit_count)
@@ -71,8 +71,10 @@ def check_for_hit_count(self, hit_count):
         if hit_count > self.hit_count_threshold:
             rospy.loginfo("tipping")
             rospy.loginfo(hit_count)
-            # publishing into tip_publisher that rover is tipping, true(1)
-            self.tip_publisher.publish(1)
+            # publishing into tip_publisher that rover is tipping, True
+            self.tip_publisher.publish(True)
+        else: # else print False
+            self.tip_publisher.publish(False)
 
     # resetting hit_count each reset_hit_count_threshold seconds
     def reset_hit_count_time(self, reset_hit_count_threshold, time_counter):

From 7dd79953d3b867259b50e4b8349a54fb75563e8e Mon Sep 17 00:00:00 2001
From: nehakankanala <nehakan@umich.edu>
Date: Sat, 24 Feb 2024 16:12:23 -0500
Subject: [PATCH 21/24] tested tip detection on the actual IMU

---
 config/esw.yaml                     |  2 +-
 launch/localization.launch          | 11 +++++++++++
 src/esw/imu_driver.py               | 28 +++++++++++++++++-----------
 src/localization/tip_detection.py   |  1 -
 src/simulator/simulator.sensors.cpp |  2 +-
 5 files changed, 30 insertions(+), 14 deletions(-)

diff --git a/config/esw.yaml b/config/esw.yaml
index 7bcd93cd6..b03084763 100644
--- a/config/esw.yaml
+++ b/config/esw.yaml
@@ -5,7 +5,7 @@ gps_driver:
   frame_id: "base_link"
 
 imu_driver:
-  port: "/dev/imu"
+  port: "/dev/tty.usbmodem101"
   baud: 115200
   frame_id: "imu_link"
 
diff --git a/launch/localization.launch b/launch/localization.launch
index 8f68cd3af..c6051a46f 100644
--- a/launch/localization.launch
+++ b/launch/localization.launch
@@ -4,6 +4,7 @@
     <arg name="ekf_start_delay" default="0" />
 
     <rosparam command="load" file="$(find mrover)/config/localization.yaml" />
+    <rosparam command="load" file="$(find mrover)/config/esw.yaml" />
     <rosparam if="$(arg sim)" command="load" file="$(find mrover)/config/sim_ekf.yaml" />
     <rosparam unless="$(arg sim)" command="load" file="$(find mrover)/config/ekf.yaml" />
 
@@ -19,5 +20,15 @@
 
     <!-- Linearize GPS from geodetic to ENU cartesian coords -->
     <node name="gps_linearization" pkg="mrover" type="gps_linearization.py" output="screen" />
+
+    <!-- Launch IMU driver -->
+    <node name="imu_driver" pkg="mrover" type="imu_driver.py" output="screen" />
+
+    <!-- Launch GPS driver -->
+    <!-- <group ns="gps">
+        <node unless="$(arg sim)" name="gps_driver" pkg="mrover" type="gps_driver.py" output="screen" />
+    </group> -->
+
     <node name="tip_detection" pkg="mrover" type="tip_detection.py" output="screen" />
+
 </launch>
\ No newline at end of file
diff --git a/src/esw/imu_driver.py b/src/esw/imu_driver.py
index 22ea8b084..09221a589 100755
--- a/src/esw/imu_driver.py
+++ b/src/esw/imu_driver.py
@@ -4,7 +4,7 @@
 from sensor_msgs.msg import Temperature, Imu, MagneticField
 from geometry_msgs.msg import Quaternion, Vector3, PoseWithCovarianceStamped, PoseWithCovariance, Vector3Stamped, Pose
 from std_msgs.msg import Header
-from mrover.msg import CalibrationStatus, ImuAndMag
+from mrover.msg import ImuAndMag
 from tf.transformations import quaternion_about_axis, quaternion_multiply, rotation_matrix, quaternion_from_matrix
 from typing import Tuple, List
 from copy import deepcopy
@@ -70,24 +70,28 @@ def main():
     and adds necessary metadata used for filtering.
     """
     orientation_covariance, gyro_covariance, accel_covariance, mag_pose_covariance = get_covariances()
-    world_frame = rospy.get_param("world_frame")
+    world_frame = rospy.get_param("global_ekf/world_frame")
 
     # publishers for all types of IMU data, queue size is 1 to make sure we don't publish old data
     imu_pub = rospy.Publisher("imu/data", ImuAndMag, queue_size=1)
     temp_pub = rospy.Publisher("imu/temp", Temperature, queue_size=1)
-    calibration_pub = rospy.Publisher("imu/calibration", CalibrationStatus, queue_size=1)
+    # calibration_pub = rospy.Publisher("imu/calibration", CalibrationStatus, queue_size=1)
     mag_pose_pub = rospy.Publisher("imu/mag_pose", PoseWithCovarianceStamped, queue_size=1)
 
     # no rate used because rate is effectively set by Arduino serial publisher
     rospy.init_node("imu_driver")
 
     # read serial connection info and IMU frame from parameter server
-    port = rospy.get_param("imu_driver/port", "/dev/imu")
+    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")
 
     # create serial connection with Arduino
     ser = serial.Serial(port, baud)
+    if ser.is_open:
+        print("Serial connection successfully opened.")
+    else:
+        print("Failed to open serial connection.")
     attempts = 0
 
     while not rospy.is_shutdown():
@@ -96,6 +100,7 @@ def main():
         try:
             line = ser.readline()
             attempts = 0
+
         except SerialException:
             attempts += 1
             if attempts > 100:
@@ -112,21 +117,22 @@ def main():
 
         # partition data into different sensors, converting calibration data from float to int
         # if the indices of data don't exist, then skip this message
+
         try:
             imu_orientation_data = data[:4]
             accel_data = data[4:7]
             gyro_data = data[7:10]
             mag_data = data[10:13]
             temp_data = data[13]
-            cal_data = [int(n) for n in data[14:18]]
+            cal_data = int(data[14])
 
         except IndexError:
             rospy.logerr("incomplete msg")
             continue
 
         # rotate the imu orientation by 90 degrees about the Z axis to convert it to ENU frame
-        enu_offset_quat = quaternion_about_axis(np.pi / 2, [0, 0, 1])
-        enu_imu_orientation = quaternion_multiply(enu_offset_quat, imu_orientation_data)
+        # enu_offset_quat = quaternion_about_axis(np.pi / 2, [0, 0, 1])
+        # enu_imu_orientation = quaternion_multiply(enu_offset_quat, imu_orientation_data)
 
         # similarly rotate the magnetometer vector into the ENU frame
         R = rotation_matrix(np.pi / 2, [0, 0, 1])
@@ -144,7 +150,7 @@ def main():
             header=header,
             imu=Imu(
                 header=header,
-                orientation=Quaternion(*enu_imu_orientation),
+                orientation=Quaternion(*imu_orientation_data),
                 linear_acceleration=Vector3(*accel_data),
                 angular_velocity=Vector3(*gyro_data),
                 orientation_covariance=orientation_covariance,
@@ -156,14 +162,14 @@ def main():
 
         temp_msg = Temperature(header=header, temperature=temp_data)
 
-        calibration_msg = CalibrationStatus(header, *cal_data)
+        # calibration_msg = CalibrationStatus(header, cal_data)
 
         # publish each message
         imu_pub.publish(imu_msg)
         temp_pub.publish(temp_msg)
-        calibration_pub.publish(calibration_msg)
+        # calibration_pub.publish(calibration_msg)
         publish_mag_pose(mag_pose_pub, mag_msg, mag_pose_covariance, world_frame)
 
 
 if __name__ == "__main__":
-    main()
+    main()
\ No newline at end of file
diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index 3432eb529..cf44b9a5a 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -49,7 +49,6 @@ def in_loop(self):
             try:
                 # extract the  matrix by [0,0,1] (z vector)
                 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]
                 self.transform = np.dot(np.array([0, 0, 1]), self.old)
 
diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp
index 39cf9e6be..b382a96a9 100644
--- a/src/simulator/simulator.sensors.cpp
+++ b/src/simulator/simulator.sensors.cpp
@@ -178,7 +178,7 @@ namespace mrover {
                 R3 imuLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast<std::chrono::duration<float>>(dt).count();
                 mRoverLinearVelocity = roverLinearVelocity;
                 SE3 imuInMap = rover.linkInWorld("imu");
-                mImuPub.publish(computeImu(imuInMap, imuAngularVelocity, imuLinearAcceleration, imuInMap.rotation().matrix().transpose().col(1)));
+                //mImuPub.publish(computeImu(imuInMap, imuAngularVelocity, imuLinearAcceleration, imuInMap.rotation().matrix().transpose().col(1)));
             }
         }
     }

From 01155040c2931258ccd039e6a1ad7ceaeb3b1566 Mon Sep 17 00:00:00 2001
From: wongau <wongau@umich.edu>
Date: Sun, 10 Mar 2024 14:04:37 -0400
Subject: [PATCH 22/24] cleaned up

---
 src/esw/imu_driver.py             | 10 ++++++++
 src/localization/tip_detection.py | 40 +++++++++++--------------------
 2 files changed, 24 insertions(+), 26 deletions(-)

diff --git a/src/esw/imu_driver.py b/src/esw/imu_driver.py
index 09221a589..5327baa33 100755
--- a/src/esw/imu_driver.py
+++ b/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)
diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index cf44b9a5a..6a62df570 100755
--- a/src/localization/tip_detection.py
+++ b/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()
 

From 66652e147ec44dae4186948af5a41aace50322d5 Mon Sep 17 00:00:00 2001
From: nehakankanala <nehakan@umich.edu>
Date: Sun, 24 Mar 2024 13:58:34 -0400
Subject: [PATCH 23/24] merge flop

---
 src/esw/imu_driver.py | 16 +++-------------
 1 file changed, 3 insertions(+), 13 deletions(-)

diff --git a/src/esw/imu_driver.py b/src/esw/imu_driver.py
index 5327baa33..2ecd6f353 100755
--- a/src/esw/imu_driver.py
+++ b/src/esw/imu_driver.py
@@ -4,7 +4,7 @@
 from sensor_msgs.msg import Temperature, Imu, MagneticField
 from geometry_msgs.msg import Quaternion, Vector3, PoseWithCovarianceStamped, PoseWithCovariance, Vector3Stamped, Pose
 from std_msgs.msg import Header
-from mrover.msg import ImuAndMag
+from mrover.msg import ImuAndMag, CalibrationStatus
 from tf.transformations import quaternion_about_axis, quaternion_multiply, rotation_matrix, quaternion_from_matrix
 from typing import Tuple, List
 from copy import deepcopy
@@ -75,22 +75,16 @@ def main():
     # publishers for all types of IMU data, queue size is 1 to make sure we don't publish old data
     imu_pub = rospy.Publisher("imu/data", ImuAndMag, queue_size=1)
     temp_pub = rospy.Publisher("imu/temp", Temperature, queue_size=1)
-    # calibration_pub = rospy.Publisher("imu/calibration", CalibrationStatus, queue_size=1)
+    calibration_pub = rospy.Publisher("imu/calibration", CalibrationStatus, queue_size=1)
     mag_pose_pub = rospy.Publisher("imu/mag_pose", PoseWithCovarianceStamped, queue_size=1)
 
     # no rate used because rate is effectively set by Arduino serial publisher
     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)
@@ -168,16 +162,12 @@ 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)
         temp_pub.publish(temp_msg)
-        # calibration_pub.publish(calibration_msg)
+        calibration_pub.publish(calibration_msg)
         publish_mag_pose(mag_pose_pub, mag_msg, mag_pose_covariance, world_frame)
 
 

From 7d8ac9a1f6c53aad40ee78764904d93ebb10e655 Mon Sep 17 00:00:00 2001
From: qhdwight <qhdwight@gmail.com>
Date: Thu, 18 Apr 2024 23:51:30 -0400
Subject: [PATCH 24/24] Format

---
 src/esw/imu_driver.py             |  2 +-
 src/localization/tip_detection.py | 10 +++++-----
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/esw/imu_driver.py b/src/esw/imu_driver.py
index 2ecd6f353..6b7bde5de 100755
--- a/src/esw/imu_driver.py
+++ b/src/esw/imu_driver.py
@@ -172,4 +172,4 @@ def main():
 
 
 if __name__ == "__main__":
-    main()
\ No newline at end of file
+    main()
diff --git a/src/localization/tip_detection.py b/src/localization/tip_detection.py
index f46023213..a6d6415a9 100755
--- a/src/localization/tip_detection.py
+++ b/src/localization/tip_detection.py
@@ -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)
 
@@ -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
@@ -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()
@@ -82,4 +83,3 @@ def main():
 
 if __name__ == "__main__":
     main()
-    
\ No newline at end of file