Tipdetection

config/esw.yaml

@@ -1,5 +1,5 @@
 imu_driver:
-  port: "/dev/imu"
+  port: "/dev/tty.usbmodem101"
   baud: 115200
   frame_id: "imu_link"
 

launch/auton.launch

@@ -0,0 +1,51 @@
+<!-- This launch file launches all nodes necessary for autonomous navigation. -->
+<launch>
+  <arg name="run_tag_detector" default="true"/>
+  <arg name="sim" default="false"/>
+  <arg name="use_ekf" default="true"/>
+  <arg name="ekf_start_delay" default="0"/>
+
+  <!--
+    ==========
+    Perception
+    ==========
+  -->
+  <node pkg="nodelet" type="nodelet" name="perception_nodelet_manager" respawn="true"
+        args="manager" output="screen"/>
+  <!-- nodelet to detect AR tags and publish them to the TF tree -->
+  <node if="$(arg run_tag_detector)"
+        pkg="nodelet" type="nodelet" name="tag_detector" respawn="true"
+        args="load mrover/TagDetectorNodelet perception_nodelet_manager" output="screen"/>
+
+  <!--
+    ===========
+    Navigation
+    ===========
+  -->
+  <!-- node to navigate to a series of waypoints, AR tags, and gates -->
+  <rosparam command="load" file="$(find mrover)/config/navigation.yaml"/>
+  <node name="nav" pkg="mrover" type="navigation.py"/>
+  <node name="failure_id" pkg="mrover" type="failure_identification.py"/>
+
+  <!--
+    ============
+    Localization
+    ============
+  -->
+  <rosparam command="load" file="$(find mrover)/config/localization.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"/>
+
+  <!-- global EKF -->
+  <node if="$(arg use_ekf)" pkg="robot_localization" type="ekf_localization_node" name="global_ekf"
+        clear_params="true" launch-prefix="bash -c 'sleep $(arg ekf_start_delay); $0 $@'">
+    <remap from="odometry/filtered" to="global_ekf/odometry"/>
+  </node>
+
+  <!-- passthrough filter to replace EKF in datapath -->
+  <node unless="$(arg use_ekf)" pkg="mrover" type="passthrough_filter.py" name="passthrough_filter"/>
+
+  <!-- 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>

launch/auton_sim.launch

@@ -0,0 +1,27 @@
+<!-- 
+    This launch file launches everything necessary to run auton code in the simulator.
+ -->
+<launch>
+    <arg name="rvizconfig" default="$(find mrover)/config/rviz/auton_sim.rviz"/>
+    <arg name="world_name" default="world"/>
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true"/>
+
+    <!-- launch rover core nodes -->
+    <include file="$(find mrover)/launch/rover_core.launch"/>
+
+    <!-- launch the simulator -->
+    <include file="$(find mrover)/launch/simulator.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+    </include>
+
+    <node pkg="tf" type="static_transform_publisher" name="zed2i_left_camera_frame_publisher"
+          args="0 0 0 0 0 0 1 left_camera_link zed2i_left_camera_frame 100"/>
+
+    <!-- launch auton -->
+    <include file="$(find mrover)/launch/auton.launch">
+        <arg name="sim" value="true"/>
+        <arg name="use_ekf" value="true"/>
+        <arg name="ekf_start_delay" value="5"/>
+    </include>
+</launch>

launch/localization.launch

@@ -30,4 +30,6 @@
         <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>

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, CalibrationStatus
 from tf.transformations import quaternion_about_axis, quaternion_multiply, rotation_matrix, quaternion_from_matrix
 from typing import Tuple, List
 from copy import deepcopy

src/localization/tip_detection.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python3
+
+import tf2_ros
+import numpy as np
+import rospy
+from std_msgs.msg import Bool
+from util.SE3 import SE3
+import time
+
+
+class TipDetection:
+    hit_count: int
+    orientation_threshold: float
+    hit_count_threshold: int
+    reset_hit_count_threshold: int
+    time_counter: time
+    current_time: time
+    tip_publisher: rospy.Publisher
+
+    def __init__(self):
+        self.tip_publisher = rospy.Publisher("tipping", Bool, queue_size=1)
+
+        self.hit_count = 0
+        self.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.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")
+        time.sleep(2)
+        self.in_loop()
+
+    def in_loop(self):
+        rate = rospy.Rate(10.0)
+        while not rospy.is_shutdown():
+            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
+            self.current_time = time.time()
+            self.reset_hit_count_time(self.reset_hit_count_threshold, self.time_counter)
+
+    # 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:
+            # publishing into tip_publisher that rover is tipping, True
+            self.tip_publisher.publish(True)
+        else:  # else publish False
+            self.tip_publisher.publish(False)
+
+    # reset hit_count each reset_hit_count_threshold seconds
+    def reset_hit_count_time(self, reset_hit_count_threshold, 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:
+            self.hit_count = 0
+            self.time_counter = time.time()
+
+
+def main():
+    rospy.init_node("tip_detection")
+    TipDetection()
+
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    main()

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

starter_project/autonomy/launch/starter_project.launch

@@ -29,7 +29,7 @@
     Perception
     ==========
   -->
-     <node name="starter_project_perception" pkg="mrover" type="starter_project_perception" />
+     <!-- <node name="starter_project_perception" pkg="mrover" type="starter_project_perception" /> -->
 
   <!--
     ===========

starter_project/autonomy/msg/StarterProjectTag.msg

@@ -0,0 +1,4 @@
+int32 tagId
+float32 xTagCenterPixel
+float32 yTagCenterPixel
+float32 closenessMetric

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():

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,15 +44,17 @@ 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]:
         """
         Retrieves the last received message regarding fid pose
         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:

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

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()