Added Script to Convert Sensor Data to ROS ".bag" Files

.gitignore

@@ -165,3 +165,9 @@ cython_debug/
 #  and can be added to the global gitignore or merged into this file.  For a more nuclear
 #  option (not recommended) you can uncomment the following to ignore the entire idea folder.
 #.idea/
+
+.vscode/
+setup/zed_sdk.run
+cuda/
+#Ignore ROS bags
+*.bag

GEMstack/scripts/convert_to_rosbag.py

@@ -0,0 +1,171 @@
+#!/usr/bin/env python3
+
+import os
+import rosbag
+import cv2
+import numpy as np
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image, PointCloud2
+import sensor_msgs.point_cloud2 as pc2
+import glob
+from datetime import datetime
+import time
+import rospy
+import std_msgs.msg  
+import argparse
+
+def get_matching_files(dir):
+    """Get files that have corresponding data in all three formats"""
+    # Get sorted lists of files
+    png_files = sorted(glob.glob(os.path.join(dir, 'color*.png')))
+    tif_files = sorted(glob.glob(os.path.join(dir, 'depth*.tif')))
+    npz_files = sorted(glob.glob(os.path.join(dir, 'lidar*.npz')))
+
+    print(f"Found {len(png_files)} PNG files")
+    print(f"Found {len(tif_files)} TIF files")
+    print(f"Found {len(npz_files)} NPZ files")
+
+    # Extract numbers from filenames
+    def get_number(filename):
+        return int(''.join(filter(str.isdigit, os.path.basename(filename))))
+
+    # Create dictionaries with numbers as keys
+    png_dict = {get_number(f): f for f in png_files}
+    tif_dict = {get_number(f): f for f in tif_files}
+    npz_dict = {get_number(f): f for f in npz_files}
+
+    # Find common numbers
+    common_numbers = set(png_dict.keys()) & set(tif_dict.keys()) & set(npz_dict.keys())
+    print(f"\nFound {len(common_numbers)} matching frame numbers")
+
+    # Get matching files in sorted order
+    common_numbers = sorted(common_numbers)
+    matching_pngs = [png_dict[n] for n in common_numbers]
+    matching_tifs = [tif_dict[n] for n in common_numbers]
+    matching_npzs = [npz_dict[n] for n in common_numbers]
+
+    return matching_pngs, matching_tifs, matching_npzs
+
+def create_rosbag_from_data(
+    dir, 
+    output_bag_path, 
+    frame_interval
+):
+    # Initialize CV bridge
+    bridge = CvBridge()
+
+    # Initialize ROS node
+    rospy.init_node('bag_creator', anonymous=True)
+
+    # Get matching files
+    png_files, tif_files, npz_files = get_matching_files(dir)
+
+    print(f"Found {len(png_files)} matching files in all three formats")
+
+    if len(png_files) == 0:
+        raise ValueError("No matching files found!")
+
+    # Create a new bag file
+    with rosbag.Bag(output_bag_path, 'w') as bag:
+        # Start time for the messages
+        start_time = rospy.Time.from_sec(time.time())
+        message_count = 0
+
+        for idx, (png_file, tif_file, npz_file) in enumerate(zip(png_files, tif_files, npz_files)):
+            # Calculate timestamp for this frame
+            timestamp = rospy.Time.from_sec(start_time.to_sec() + idx * (1/frame_interval))
+
+            try:
+                # Process PNG image
+                png_img = cv2.imread(png_file)
+                if png_img is not None:
+                    png_msg = bridge.cv2_to_imgmsg(png_img, encoding="bgr8")
+                    png_msg.header.stamp = timestamp
+                    png_msg.header.frame_id = "camera_rgb"
+                    bag.write('/camera/rgb/image_raw', png_msg, timestamp)
+                    message_count += 1
+                else:
+                    print(f"Warning: Could not read PNG file: {png_file}")
+
+                # Process TIF image
+                tif_img = cv2.imread(tif_file, -1)  # -1 to preserve original depth
+                if tif_img is not None:
+                    tif_msg = bridge.cv2_to_imgmsg(tif_img, encoding="passthrough")
+                    tif_msg.header.stamp = timestamp
+                    tif_msg.header.frame_id = "camera_depth"
+                    bag.write('/camera/depth/image_raw', tif_msg, timestamp)
+                    message_count += 1
+                else:
+                    print(f"Warning: Could not read TIF file: {tif_file}")
+
+                # Process pointcloud NPZ
+                pc_data = np.load(npz_file)
+                points = pc_data['arr_0']  # Using 'arr_0' based on the provided files'
+
+                # Create pointcloud message
+                header = std_msgs.msg.Header()
+                header.stamp = timestamp
+                header.frame_id = "velodyne"
+
+                fields = [
+                    pc2.PointField('x', 0, pc2.PointField.FLOAT32, 1),
+                    pc2.PointField('y', 4, pc2.PointField.FLOAT32, 1),
+                    pc2.PointField('z', 8, pc2.PointField.FLOAT32, 1)
+                ]
+
+                pc_msg = pc2.create_cloud(header, fields, points)
+                bag.write('/velodyne_points', pc_msg, timestamp)
+                message_count += 1
+
+            except Exception as e:
+                print(f"Error processing frame {idx}: {str(e)}")
+                continue
+
+            if idx % 10 == 0:
+                print(f"Processed {idx} frames")
+
+        print(f"Total messages written to bag: {message_count}")
+
+if __name__ == "__main__":
+    #This initializes the parser and sets the parameters that the user will be asked to provide in the terminal
+    parser = argparse.ArgumentParser(
+        description='A script to convert data gathered by cameras and lidar sensors to ros .bag messages'
+    )
+
+    parser.add_argument(
+        'files_directory', type = str,
+        help = 'The path to the directory with all the rgb images, depth maps, and point clouds. The file formats must be PNG, TIF, and NPZ, respectively'
+    )
+
+    parser.add_argument(
+        'output_bag', type = str,
+        help = 'The path to the directory where the bag file will be saved'
+    )
+
+    parser.add_argument(
+        'rate', type = int,
+        help = 'The rate at which the data is collected in Hz'
+    )
+    args = parser.parse_args()
+    directory = args.files_directory
+    output_bag = args.output_bag
+    rate = args.rate
+    # Example directories below:
+    #directory = "/home/username/host/CS588/GEMstack/data/data_sample/data/"
+    #output_bag = "/home/username/host/CS588/GEMstack/data/data_sample/data/output.bag"
+
+    try:
+        create_rosbag_from_data(
+            directory,
+            output_bag,
+            rate
+        )
+        print("Successfully created ROS bag file!")
+
+        # Verify bag contents
+        print("\nBag file contents:")
+        info_cmd = f"rosbag info {output_bag}"
+        os.system(info_cmd)
+
+    except Exception as e:
+        print(f"Error creating ROS bag: {str(e)}")

README.md

@@ -17,19 +17,79 @@ You should also have the following Python dependencies installed, which you can
 - matplotlib
 - opencv-python
 - torch
-- klampt
+- klampt==0.9.2
 - shapely
 - dacite
 - pyyaml
 
 
 In order to interface with the actual GEM e2 vehicle, you will need [PACMOD2](https://github.com/astuff/pacmod2) - Autonomoustuff's low level interface to vehicle. You will also need Autonomoustuff's [sensor message packages](https://github.com/astuff/astuff_sensor_msgs).  The onboard computer uses Ubuntu 20.04 with Python 3.8, CUDA 11.6, and NVIDIA driver 515, so to minimize compatibility issues you should ensure that these are installed on your development system.
 
-From a fresh Ubuntu 20.04 with ROS Noetic and [CUDA 11.6 installed](https://gist.github.com/ksopyla/bf74e8ce2683460d8de6e0dc389fc7f5), you can install these dependencies by running `setup/setup_this_machine.sh` from the top-level GEMstack folder.
+## Running the stack on Ubuntu 20.04 without Docker
+### Checking CUDA Version
 
-To build a Docker container with all these prerequisites, you can use the provided Dockerfile by running `docker build -t gem_stack setup/`.  For GPU support you will need the NVidia Container Runtime (run `setup/get_nvidia_container.sh` from this directory to install, or see [this tutorial](https://collabnix.com/introducing-new-docker-cli-api-support-for-nvidia-gpus-under-docker-engine-19-03-0-beta-release/) to install) and run `docker run -it --gpus all gem_stack /bin/bash`.
+Before proceeding, check your Nvidia Driver and supported CUDA version:
+```bash
+nvidia-smi
+```
+This will show your NVIDIA driver version and the maximum supported CUDA version. Make sure you have CUDA 11.8 or 12+ installed.
+
+From Ubuntu 20.04 install [CUDA 11.6](https://gist.github.com/ksopyla/bf74e8ce2683460d8de6e0dc389fc7f5) or [CUDA 12+](https://gist.github.com/ksopyla/ee744bf013c83e4aa3fc525634d893c9) based on your current Nvidia Driver versio.
+
+To check the currently installed CUDA version:
+```bash
+nvcc --version
+```
+you can install the dependencies or GEMstack by running `setup/setup_this_machine.sh` from the top-level GEMstack folder.
+
+## Running the stack on Ubuntu 20.04 or 22.04 with Docker
+> [!NOTE]
+> Make sure to check the Nvidia Driver and supported CUDA version before proceeding by following the steps in the previous section.
+
+For GPU support you will need the NVidia Container Toolkit (run `setup/get_nvidia_container.sh` from this directory to install, or see [this](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html) for more details).
 
+## Building the Docker image
 
+To build a Docker image with all these prerequisites, you can use the provided Dockerfile by running.
+
+```bash
+bash setup/build_docker_image.sh
+```
+
+## Running the Docker container
+
+To run the container, you can use the provided Docker Compose file by running.
+> [!NOTE]
+> If you want to open multiple terminals to run the container, you can use the same command. It will automatically start a new terminal inside the same container.
+```bash
+bash run_docker_container.sh
+```
+## Usage Tips and Instructions
+
+### Using Host Volume
+
+You can use the host volume under the container's home directory inside the `<username>` folder. This allows you to build and run files that are on the host machine. For example, if you have a file on the host machine at `/home/<username>/project`, you can access it inside the container at `/home/<username>/host/project`.
+
+### Using Dev Containers Extension in VSCode
+
+To have a good developer environment inside the Docker container, you can use the Dev Containers extension in VSCode. Follow these steps:
+
+1. Install the Dev Containers extension in VSCode.
+2. Open the cloned repository in VSCode.
+3. Press `ctrl+shift+p`(or select the remote explorer icon from the left bar) and select `Dev-Containers: Attach to Running Container...`.
+4. Select the container name `gem_stack-container`.
+5. Once attached, Select `File->Open Folder...`.
+6. Select the folder/workspace you want to open in the container.
+
+This will set up the development environment inside the Docker container, allowing you to use VSCode features seamlessly.
+
+## Stopping the Docker container
+
+To stop the container, you can use the provided stop script by running.
+
+```bash
+bash stop_docker_container.sh
+```
 
 ## In this folder
 

homework/longitudinal_planning.py

@@ -77,7 +77,11 @@ def update(self, state : AllState):
         #parse the relations indicated
         should_brake = False
         for r in state.relations:
+<<<<<<< HEAD
+            if r.type == EntityRelationEnum.YIELD and r.obj1 == '':
+=======
             if r.type == EntityRelationEnum.YIELDING and r.obj1 == '':
+>>>>>>> infra_a_main
                 #yielding to something, brake
                 should_brake = True
         should_accelerate = (not should_brake and curr_v < self.desired_speed)

requirements.txt

@@ -4,6 +4,6 @@ scipy
 matplotlib
 torch
 shapely
-klampt
+klampt==0.9.2
 pyyaml
 dacite

run_docker_container.sh

@@ -0,0 +1,8 @@
+#!/bin/bash
+# Check if container is already running
+if [ "$(docker ps -q -f name=gem_stack-container)" ]; then
+    docker exec -it gem_stack-container bash
+else
+    docker compose -f setup/docker-compose.yaml up -d
+    docker exec -it gem_stack-container bash
+fi

setup/Dockerfile → setup/Dockerfile.cuda11.8

@@ -1,7 +1,28 @@
 FROM nvidia/cuda:11.8.0-cudnn8-devel-ubuntu20.04
+
+ARG USER_UID=1000
+ARG USER_GID=1000
+ARG USER=$(whoami)
+
+ENV DEBIAN_FRONTEND=noninteractive
+
 #use bash instead of sh
-RUN rm /bin/sh && ln -s /bin/bash /bin/sh
+SHELL ["/bin/bash", "-c"]
+
 RUN apt-get update && apt-get install -y git python3 python3-pip wget zstd
+
+# Set time zone non-interactively
+ENV TZ=America/Chicago
+RUN ln -fs /usr/share/zoneinfo/$TZ /etc/localtime \
+    && echo $TZ > /etc/timezone \
+    && apt-get update && apt-get install -y tzdata \
+    && rm -rf /var/lib/apt/lists/*
+
+# install Zed SDK
+RUN wget https://download.stereolabs.com/zedsdk/4.0/cu118/ubuntu20 -O zed_sdk.run
+RUN chmod +x zed_sdk.run
+RUN ./zed_sdk.run -- silent
+
 # Install ROS Noetic
 RUN apt-get update && apt-get install -y lsb-release gnupg2
 RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
@@ -13,19 +34,18 @@ RUN apt-get install -y python3-rosdep python3-rosinstall python3-rosinstall-gene
 RUN rosdep init
 RUN rosdep update
 
-#Install Cuda 11.8
-#RUN wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-ubuntu2004.pin
-#RUN sudo mv cuda-ubuntu2004.pin /etc/apt/preferences.d/cuda-repository-pin-600
-##add public keys
-#RUN sudo apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/3bf863cc.pub
-#RUN sudo add-apt-repository "deb http://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/ /"
-#RUN install cuda-toolkit-11-8
+ARG USER
+ARG USER_UID
+ARG USER_GID
 
+# Create user more safely
+RUN groupadd -g ${USER_GID} ${USER} || groupmod -n ${USER} $(getent group ${USER_GID} | cut -d: -f1)
+RUN useradd -l -m -u ${USER_UID} -g ${USER_GID} ${USER} || usermod -l ${USER} -m -u ${USER_UID} -g ${USER_GID} $(getent passwd ${USER_UID} | cut -d: -f1)
+RUN echo "${USER} ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers
 
-# install Zed SDK
-RUN wget https://download.stereolabs.com/zedsdk/4.0/cu118/ubuntu20 -O zed_sdk.run
-RUN chmod +x zed_sdk.run
-RUN ./zed_sdk.run -- silent
+# Fix permissions for Python packages
+RUN chown -R ${USER}:${USER} /usr/local/lib/python3.8/dist-packages/ \
+    && chmod -R u+rw /usr/local/lib/python3.8/dist-packages/
 
 # create ROS Catkin workspace
 RUN mkdir -p /catkin_ws/src
@@ -42,8 +62,19 @@ RUN cd /catkin_ws/src && git clone https://github.com/ros-perception/radar_msgs.
 RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && rosdep install --from-paths src --ignore-src -r -y
 RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && catkin_make -DCMAKE_BUILD_TYPE=Release
 
-# install GEMstack Python dependencies
-RUN git clone https://github.com/krishauser/GEMstack.git
-RUN cd GEMstack && pip3 install -r requirements.txt
+# Copy requirements.txt from host (now relative to parent directory)
+COPY requirements.txt /tmp/requirements.txt
+
+# Install Python dependencies
+RUN pip3 install -r /tmp/requirements.txt
+
+USER ${USER}
+
+# Add ROS and GEMstack paths to bashrc
+RUN echo "source /opt/ros/noetic/setup.bash" >> /home/${USER}/.bashrc
+RUN echo "source /catkin_ws/devel/setup.bash" >> /home/${USER}/.bashrc
+
+# BASE END CONFIG
+WORKDIR /home/${USER}
 
-RUN echo /catkin_ws/devel/setup.sh
+ENTRYPOINT ["/bin/bash"]