Mockrolls 19 09 24

camera.py

@@ -0,0 +1,70 @@
+########################################################################
+#
+# Copyright (c) 2022, STEREOLABS.
+#
+# All rights reserved.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+########################################################################
+
+import sys
+from signal import signal, SIGINT
+import argparse
+
+
+import pyzed.sl as sl
+
+cam = sl.Camera()
+
+#Handler to deal with CTRL+C properly
+def handler(signal_received, frame):
+    cam.disable_recording()
+    cam.close()
+    sys.exit(0)
+
+signal(SIGINT, handler)
+
+def main():
+
+    init = sl.InitParameters()
+    init.depth_mode = sl.DEPTH_MODE.NONE # Set configuration parameters for the ZED
+
+    status = cam.open(init)
+    if status != sl.ERROR_CODE.SUCCESS:
+        print("Camera Open", status, "Exit program.")
+        exit(1)
+
+    recording_param = sl.RecordingParameters(opt.output_svo_file, sl.SVO_COMPRESSION_MODE.H264) # Enable recording with the filename specified in argument
+    err = cam.enable_recording(recording_param)
+    if err != sl.ERROR_CODE.SUCCESS:
+        print("Recording ZED : ", err)
+        exit(1)
+
+    runtime = sl.RuntimeParameters()
+    print("SVO is Recording, use Ctrl-C to stop.") # Start recording SVO, stop with Ctrl-C command
+    frames_recorded = 0
+
+    while True:
+        if cam.grab(runtime) == sl.ERROR_CODE.SUCCESS : # Check that a new image is successfully acquired
+            frames_recorded += 1
+            print("Frame count: " + str(frames_recorded), end="\r")
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--output_svo_file', type=str, help='Path to the SVO file that will be written', required= True)
+    opt = parser.parse_args()
+    if not opt.output_svo_file.endswith(".svo") and not opt.output_svo_file.endswith(".svo2"):
+        print("--output_svo_file parameter should be a .svo file but is not : ",opt.output_svo_file,"Exit program.")
+        exit()
+    main()

docker_auton/Dockerfile

@@ -1,8 +1,8 @@
-FROM nvidia/cuda:11.6.2-base-ubuntu20.04 as CUDA
+# FROM nvidia/cuda:11.6.2-base-ubuntu20.04 as CUDA
 
 FROM osrf/ros:noetic-desktop-full-focal
 
-COPY --from=CUDA /usr/local/cuda /usr/local/
+# COPY --from=CUDA /usr/local/cuda /usr/local/
 
 
 RUN apt update
@@ -16,10 +16,11 @@ RUN apt-get install -y -qq \
   ros-noetic-foxglove-bridge \
   ros-noetic-microstrain-inertial-driver \
   ros-noetic-realsense2-camera \
-  ros-noetic-realsense2-description
+  ros-noetic-realsense2-description \
+  ros-${ROS_DISTRO}-mavros ros-${ROS_DISTRO}-mavros-extras ros-${ROS_DISTRO}-mavros-msgs
 
 # Run this now to cache it separately from other requirements
-COPY cuda-requirements_TEMP_DO_NOT_EDIT.txt cuda-requirements.txt
+# COPY cuda-requirements_TEMP_DO_NOT_EDIT.txt cuda-requirements.txt
 # RUN pip3 install -r cuda-requirements.txt
 
 

python-requirements.txt

@@ -1,4 +1,5 @@
 matplotlib==3.1.2
+NavPy==1.0
 numba==0.58.0
 numpy<1.21.0
 osqp==0.6.3

rb_ws/src/buggy/launch/debug_filter.launch

@@ -9,7 +9,7 @@
         <arg name="params_file" value="/rb_ws/src/buggy/INS_params.yml"/>
     </include>
 
-    <remap from="/SC/nav/odom" to="/nav/odom"/>
+    <remap from="/SC/nav/odom" to="/ekf/odometry_earth"/>
     <!-- <remap from="/buggy/input/steering" to="/SC/buggy/input/steering"/> -->
 
     <node name="serial_node" pkg="rosserial_python" type="serial_node.py">

rb_ws/src/buggy/launch/ghost_auton.launch

@@ -14,7 +14,7 @@
         args="$(arg starting_pose) $(arg velocity) $(arg buggy_name)"/>
 
 
-    <remap from="/SC/nav/odom" to="/nav/odom"/>
+    <remap from="/SC/nav/odom" to="/ekf/odometry_earth"/>
 
     <!-- Run the simulation with NAND -->
     <arg name="autonsystem_args" default="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --other_name NAND" />

rb_ws/src/buggy/launch/sc-main.launch

@@ -3,9 +3,9 @@
 <launch>
     <arg name="controller"      default="stanley" />
     <arg name="start_dist"      default="0.0" />
-    <arg name="path"      default="buggycourse_safe_1.json" />
+    <arg name="path"      default="buggycourse_raceline.json" />
 
-    <remap from="/SC/nav/odom" to="/nav/odom"/>
+    <!-- <remap from="/SC/nav/odom" to="/ekf/odometry_earth"/> -->
     <node name="sc_rolling_sanity_check" pkg="buggy" type="rolling_sanity_check.py" output="screen" args="SC"/>
 
     <!-- ENABLE AUTON -->
@@ -14,11 +14,11 @@
     <arg name="NAND_exist" default="true"/>
     <group if="$(arg NAND_exist)">
         <!-- Run the simulation with NAND -->
-        <arg name="autonsystem_args" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --other_name NAND --left_curb curb.json" />
+        <arg name="autonsystem_args" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --other_name NAND  --left_curb buggycourse_curb.json" />
         <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg autonsystem_args)"/>
     </group>
     <group unless="$(arg NAND_exist)">
-        <arg name="autonsystem_args" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --left_curb curb.json" />
+        <arg name="autonsystem_args" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC  --left_curb buggycourse_curb.json" />
         <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg autonsystem_args)"/>
     </group>
 </launch>

rb_ws/src/buggy/launch/sc-system.launch

@@ -2,7 +2,7 @@
 
 <launch>
     <include file="$(find microstrain_inertial_driver)/launch/microstrain.launch">
-        <arg name="params_file" value="/rb_ws/src/buggy/INS_params.yml"/>
+        <arg name="params_file" value="/rb_ws/src/buggy/INS_params_v2.yml"/>
     </include>
 
     <node name="bnyahaj" pkg="buggy" type="ros_to_bnyahaj.py" respawn="true" output="screen" args="--self_name SC --other_name NAND --teensy_name ttyUSB0"/>

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

@@ -21,6 +21,8 @@
 from path_planner import PathPlanner
 from pose import Pose
 
+import navpy
+
 class AutonSystem:
     """
     Top-level class for the RoboBuggy autonomous system
@@ -68,6 +70,10 @@ def __init__(self,
         self.other_odom_msg = None
         self.use_gps_pos = False
 
+        #TODO: DOUBLE CONVERTING HERE, NOT A GOOD IDEA
+        rospy.Subscriber("/ekf/odometry_earth", Odometry, self.change_utm_latlon)
+        self.latlonodom = rospy.Publisher(self_name + "/nav/odom", Odometry, queue_size=1)
+
         rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_self_odom)
         rospy.Subscriber(self_name + "/gnss1/odom", Odometry, self.update_self_odom_backup)
         rospy.Subscriber(self_name + "/nav/traj", TrajectoryMsg, self.update_traj)
@@ -103,6 +109,12 @@ def __init__(self,
         self.profile = profile
         self.tick_caller()
 
+    def change_utm_latlon(self, msg):
+        new_msg = msg
+        lat, lon, _ = navpy.ecef2lla([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+        new_msg.pose.pose.position.x, new_msg.pose.pose.position.y = lon, lat
+        self.latlonodom.publish(new_msg)
+
 
     # functions to read data from ROS nodes
     def update_use_gps(self, msg):

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

@@ -13,6 +13,8 @@
 import rospy
 
 
+#BROKEN TOPIC PATHS, INCREDIBLY BROKEN
+
 class BuggyState:
     """
     Basically a translator from ROSOdom to ensure everything is in the correct units.

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

@@ -10,6 +10,8 @@
 from pose import Pose
 from world import World
 
+
+#BROKEN BROKEN BROKEN BROKEN
 class RTKErrPublisher():
 
     """

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

@@ -6,12 +6,14 @@
 
 from nav_msgs.msg import Odometry
 from std_msgs.msg import Bool, String, Int8MultiArray, Int8
-from microstrain_inertial_msgs.msg import FilterStatus, ImuOverrangeStatus
+from microstrain_inertial_msgs.msg import MipFilterStatus, ImuOverrangeStatus
 from geometry_msgs.msg import PoseStamped
 
 from world import World
 from pose import Pose
 
+#BROKEN BROKEN BROKEN
+
 class SanityCheck:
     """
         Wrapper to publish a lot of error and debug data regarding the estimated position of the specified buggy and INS output
@@ -47,7 +49,7 @@ def __init__(self,
 
 
         rospy.Subscriber(self_name + "/imu/overrange_status", ImuOverrangeStatus, self.update_overrange_status)
-        rospy.Subscriber(self_name + "/nav/status.status_flags", FilterStatus, self.update_status_flags)
+        rospy.Subscriber(self_name + "/nav/status.status_flags", MipFilterStatus, self.update_status_flags)
         rospy.Subscriber(self_name + "/gnss1/fix_Pose/", PoseStamped, self.update_gps_location)
         rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_filter_location)
 
@@ -81,7 +83,7 @@ def __init__(self,
     def update_overrange_status(self, msg : ImuOverrangeStatus):
         self.imu_overrange_status = msg.status
 
-    def update_status_flags(self, msg : FilterStatus):
+    def update_status_flags(self, msg : MipFilterStatus):
         self.status_flag_val = msg.gq7_status_flags
 
     def update_gps_location(self, msg : PoseStamped):

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

@@ -4,6 +4,7 @@
 import rosbag
 from tf.transformations import euler_from_quaternion
 
+#BROKEN BROKEN BROKEN BROKEN
 
 def main():
     """

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

@@ -5,6 +5,8 @@
 
 import rosbag
 
+#BROKEN BROKEN BROKEN
+
 def main():
     # Read in bag path from command line
     parser = argparse.ArgumentParser()

rb_ws/src/buggy/scripts/visualization/telematics.py

@@ -8,7 +8,7 @@
 from std_msgs.msg import String
 from std_msgs.msg import Int8
 
-from microstrain_inertial_msgs.msg import GNSSFixInfo
+from microstrain_inertial_msgs.msg import MipGnssFixInfo
 
 class Telematics:
     """
@@ -24,19 +24,19 @@ def __init__(self):
 
         self.gnss1_pose_publisher = rospy.Publisher("/gnss1/fix_Pose", PoseStamped, queue_size=10)
         self.gnss1_covariance_publisher = rospy.Publisher("/gnss1/fix_FloatArray_Covariance", Float64MultiArray, queue_size=10)
-        self.gnss1_subscriber = rospy.Subscriber("/gnss1/fix", NavSatFix, self.convert_navsatfix_to_pose_covariance, (self.gnss1_pose_publisher, self.gnss1_covariance_publisher))
+        self.gnss1_subscriber = rospy.Subscriber("/gnss1/llh_position", NavSatFix, self.convert_navsatfix_to_pose_covariance, (self.gnss1_pose_publisher, self.gnss1_covariance_publisher))
 
         self.gnss2_pose_publisher = rospy.Publisher("/gnss2/fix_Pose", PoseStamped, queue_size=10)
         self.gnss2_covariance_publisher = rospy.Publisher("/gnss2/fix_FloatArray_Covariance", Float64MultiArray, queue_size=10)
-        self.gnss2_subscriber = rospy.Subscriber("/gnss2/fix", NavSatFix, self.convert_navsatfix_to_pose_covariance, (self.gnss2_pose_publisher, self.gnss2_covariance_publisher))
+        self.gnss2_subscriber = rospy.Subscriber("/gnss2/llh_position", NavSatFix, self.convert_navsatfix_to_pose_covariance, (self.gnss2_pose_publisher, self.gnss2_covariance_publisher))
 
         self.gnss1_fixinfo_publisher = rospy.Publisher("/gnss1/fix_info_republished", String, queue_size=10)
         self.gnss1_fixinfo_int_publisher = rospy.Publisher("/gnss1/fix_info_republished_int", Int8, queue_size=10)
-        self.gnss1_fixinfo_subscriber = rospy.Subscriber("/gnss1/fix_info", GNSSFixInfo, self.republish_fixinfo, (self.gnss1_fixinfo_publisher, self.gnss1_fixinfo_int_publisher))
+        self.gnss1_fixinfo_subscriber = rospy.Subscriber("/mip/gnss1/fix_info", MipGnssFixInfo, self.republish_fixinfo, (self.gnss1_fixinfo_publisher, self.gnss1_fixinfo_int_publisher))
 
         self.gnss2_fixinfo_publisher = rospy.Publisher("/gnss2/fix_info_republished", String, queue_size=10)
         self.gnss2_fixinfo_int_publisher = rospy.Publisher("/gnss2/fix_info_republished_int", Int8, queue_size=10)
-        self.gnss2_fixinfo_subscriber = rospy.Subscriber("/gnss2/fix_info", GNSSFixInfo, self.republish_fixinfo, (self.gnss2_fixinfo_publisher, self.gnss2_fixinfo_int_publisher))
+        self.gnss2_fixinfo_subscriber = rospy.Subscriber("/mip/gnss2/fix_info", MipGnssFixInfo, self.republish_fixinfo, (self.gnss2_fixinfo_publisher, self.gnss2_fixinfo_int_publisher))
 
     def convert_odometry_to_navsatfix(self, msg):
         """Convert Odometry-type to NavSatFix-type for plotting on Foxglove

telematics_layout.json

@@ -5,7 +5,7 @@
       "diffMethod": "custom",
       "diffTopicPath": "",
       "showFullMessageForDiff": false,
-      "topicPath": "/nav/dual_antenna_status"
+      "topicPath": "/mip/ekf/gnss_dual_antenna_status"
     },
     "RawMessages!3xary91": {
       "diffEnabled": false,
@@ -94,8 +94,8 @@
       "followTopic": "",
       "layer": "map",
       "topicColors": {
-        "/gnss1/fix": "#e34f4f",
-        "/gnss2/fix": "#4fd5e3"
+        "/gnss1/llh_position": "#e34f4f",
+        "/gnss2/llh_position": "#4fd5e3"
       },
       "zoomLevel": 16,
       "maxNativeZoom": 20
@@ -152,7 +152,7 @@
       "diffMethod": "custom",
       "diffTopicPath": "",
       "showFullMessageForDiff": false,
-      "topicPath": "/gnss2/fix_info",
+      "topicPath": "/mip/gnss2/fix_info",
       "expansion": "none"
     }
   },