From 319e53e98cf533ecc113433ed0e24e4dd744abc8 Mon Sep 17 00:00:00 2001 From: Buggy Date: Fri, 20 Sep 2024 00:06:53 -0400 Subject: [PATCH] Mock rolls 09 - 19 --- camera.py | 69 +++++++++++++++++++ docker_auton/Dockerfile | 9 +-- .../scripts/auton/rolling_sanity_check.py | 6 +- .../buggy/scripts/visualization/telematics.py | 6 +- 4 files changed, 80 insertions(+), 10 deletions(-) create mode 100644 camera.py diff --git a/camera.py b/camera.py new file mode 100644 index 00000000..aa3c8933 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/docker_auton/Dockerfile b/docker_auton/Dockerfile index 2804440c..a05558e6 100755 --- a/docker_auton/Dockerfile +++ b/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 diff --git a/rb_ws/src/buggy/scripts/auton/rolling_sanity_check.py b/rb_ws/src/buggy/scripts/auton/rolling_sanity_check.py index eca106aa..c6e4b624 100644 --- a/rb_ws/src/buggy/scripts/auton/rolling_sanity_check.py +++ b/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): diff --git a/rb_ws/src/buggy/scripts/visualization/telematics.py b/rb_ws/src/buggy/scripts/visualization/telematics.py index 182b2743..8c147ffb 100755 --- a/rb_ws/src/buggy/scripts/visualization/telematics.py +++ b/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