implementationOfLocalEKF

docker_auton/Dockerfile

@@ -9,7 +9,8 @@ RUN apt update
 RUN apt-get install -y -qq \
   python3-pip \
   python3-tk \
-  vim git tmux tree sl htop x11-apps
+  vim git tmux tree sl htop x11-apps \
+  python-is-python3
 
 RUN apt-get install -y -qq \
   ros-noetic-rosserial \

rb_ws/src/buggy/launch/ekf.launch

@@ -10,21 +10,23 @@
     <arg name="auto_vel"            default="false" />
 
     <!-- <node pkg="tf" type="static_transform_publisher" name="static_transform" args="0 0 0 0 0 0  /base_footprint /base_link  100"/> -->
+    <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 1 /imu /base_footprint  10 " />
+
+    <node name="ekf_translator" pkg="buggy" type="ekf.py" respawn="true" output="screen" />  
 
     <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
         <param name="output_frame" value="odom_combined"/>
-        <param name="base_footprint_frame" value="base_footprint"/>
-        <param name="freq" value="30.0"/>
+        <param name="freq" value="30000000.0"/>
         <param name="sensor_timeout" value="1.0"/>  
         <param name="odom_used" value="true"/>
         <param name="imu_used" value="true"/>
         <param name="vo_used" value="false"/>
         <param name="gps_used" value="false"/>
-        <param name="debug" value="true"/>
-        <param name="self_diagnose" value="true"/>
+        <param name="debug" value="false"/>
+        <param name="self_diagnose" value="false"/>
 
         <remap from="imu_data" to="/imu/data"/>
+        <remap from="gps" to="/nav/odom"/>
         <remap from="odom" to="/nav/odom"/>
-  </node>
-
+    </node>
 </launch>

rb_ws/src/buggy/scripts/auton/buggystate.py

@@ -6,7 +6,7 @@
 
 from std_msgs.msg import Bool
 
-from microstrain_inertial_msgs.msg import GNSSFixInfo
+# from microstrain_inertial_msgs.msg import GNSSFixInfo
 
 
 
@@ -52,21 +52,6 @@ def update_gnss2(self, msg):
     def update_gnss1_fix(self, msg):
         self.gps_fix = msg.fix_type
 
-    def odom_to_navsatfix(self, odom):
-        """Convert Odometry-type to NavSatFix-type for plotting on Foxglove
-        Args:
-            odom (Odometry): odometry object to convert
-        """
-        lat = odom.pose.pose.position.y
-        long = odom.pose.pose.position.x
-        down = odom.pose.pose.position.z
-        new_odom = NavSatFix()
-        new_odom.header = odom.header
-        new_odom.latitude = lat
-        new_odom.longitude = long
-        new_odom.altitude = down
-        return new_odom
-
     def get_gps_fix(self):
         fix_string = "fix type: "
         if (self.gps_fix == 0):
@@ -113,3 +98,18 @@ def get_velocity(self):
             return None
         return self.filter_odom.twist.twist.linear
 
+
+    def odom_to_navsatfix(odom):
+        """Convert Odometry-type to NavSatFix-type for plotting on Foxglove
+        Args:
+            odom (Odometry): odometry object to convert
+        """
+        lat = odom.pose.pose.position.y
+        long = odom.pose.pose.position.x
+        down = odom.pose.pose.position.z
+        new_odom = NavSatFix()
+        new_odom.header = odom.header
+        new_odom.latitude = lat
+        new_odom.longitude = long
+        new_odom.altitude = down
+        return new_odom

rb_ws/src/buggy/scripts/util/ekf.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python3
+
+import sys
+
+# Allows import of world and pose from auton directory
+# sys.path.append("/rb_ws/src/buggy/scripts/auton")
+
+# from buggystate import BuggyState
+from threading import Lock
+import rospy
+
+# Ros Message Imports
+from geometry_msgs.msg import PoseWithCovarianceStamped as ROSOdom
+from sensor_msgs.msg import NavSatFix
+
+
+class ekfTranslator:
+    def __init__(self):
+        """
+        self_name: Buggy namespace
+        other_name: Only requried by SC for passing buggy namespace
+        teensy_name: required for communication, different for SC and NAND
+
+        Initializes the subscribers, rates, and ros topics (including debug topics)
+        """
+        self.odomMsg = None
+
+        rospy.Subscriber(
+            "/robot_pose_ekf/odom_combined", ROSOdom, self.set_odom
+        )
+
+        self.lock = Lock()
+
+        self.publisher = rospy.Publisher("/robot_pose_ekf/navsatfix", NavSatFix, queue_size=1)
+
+    def set_odom(self, msg):
+        with self.lock:
+            lat = msg.pose.pose.position.y
+            long = msg.pose.pose.position.x
+            down = msg.pose.pose.position.z
+            new_odom = NavSatFix()
+            new_odom.header = msg.header
+            new_odom.latitude = lat
+            new_odom.longitude = long
+            new_odom.altitude = down
+            self.publisher.publish(new_odom)
+            print("ekf Translated")
+
+
+if __name__ == "__main__":
+    rospy.init_node("ekf_translator")
+    translate = ekfTranslator()
+    print("Initialized EKF Translator")
+    x = 1
+    while True:
+        x+=1