Mock rolls 09 - 19

camera.py

@@ -0,0 +1,69 @@
+########################################################################
+#
+# 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
+import pyzed.sl as sl
+from signal import signal, SIGINT
+import argparse 
+import os 
+
+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
 
 

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

@@ -6,7 +6,7 @@
 
 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
@@ -47,7 +47,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 +81,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/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:
     """
@@ -32,11 +32,11 @@ def __init__(self):
 
         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("/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("/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