From cffde30027dfd778f374bac029bc3fa1449407c0 Mon Sep 17 00:00:00 2001 From: krishauser Date: Mon, 27 Jan 2025 14:32:38 -0500 Subject: [PATCH 1/9] Added homework description and starter code --- HOMEWORK.md | 194 +++++++++++++++++++++++++++++ homework/blink.py | 81 ++++++++++++ homework/blink_component.py | 39 ++++++ homework/blink_launch.yaml | 58 +++++++++ homework/longitudinal_planning.py | 93 ++++++++++++++ homework/pedestrian_detection.py | 84 +++++++++++++ homework/pedestrian_detection.yaml | 97 +++++++++++++++ homework/pedestrian_yield_logic.py | 22 ++++ homework/person_detector.py | 48 +++++++ homework/test_longitudinal_plan.py | 80 ++++++++++++ 10 files changed, 796 insertions(+) create mode 100644 HOMEWORK.md create mode 100644 homework/blink.py create mode 100644 homework/blink_component.py create mode 100644 homework/blink_launch.yaml create mode 100644 homework/longitudinal_planning.py create mode 100644 homework/pedestrian_detection.py create mode 100644 homework/pedestrian_detection.yaml create mode 100644 homework/pedestrian_yield_logic.py create mode 100644 homework/person_detector.py create mode 100644 homework/test_longitudinal_plan.py diff --git a/HOMEWORK.md b/HOMEWORK.md new file mode 100644 index 000000000..144c57282 --- /dev/null +++ b/HOMEWORK.md @@ -0,0 +1,194 @@ +# CS 588 Phase 1 Homework + +Due date: 2/24 + +**Overall objective**: work together in large teams to create a nontrivial integrated behavior on the vehicle. The target behavior will be to drive along a predefined track, and slowing down for pedestrians that would otherwise be hit if the vehicle continues at normal speed. The vehicle should stop when necessary. + +**Skills**: Familiarization with GEMstack repo, Git branches and pull requests, lab and equipment logistics, inter-team communication. + +**On-board development**: If you are working on the GEM vehicle, your work will go into the `cs588_groupX/` folder. We recommend that before you start, you +a) Ensure that your GEMstack repository is on the correct branch. cd into `cs588_groupX/GEMstack` and run `git status`. It should show that you are on the `s2025_groupX` branch. +b) Get the latest updates to the course stack. To do so, run `git pull` and `git merge s2025`. + +When you are done, commit your development of this component and launch files to your group's branch. **DO NOT** set the global git authentication on the vehicle to your account. The easiest way to do this is: +a) Log onto Github under your account name. [Create a Github Personal Access Token](https://docs.github.com/en/authentication/keeping-your-account-and-data-secure/managing-your-personal-access-tokens#creating-a-personal-access-token-classic). To reduce the chance of mischief, choose the "Only select repositories" option and set the access token just to push to this repository. Open the "Repository permissions" and change the "Contents" access to "Read and write" +b) When your token is generated, copy the token and keep it somewhere safe. +c) Commit your changes to your branch. +d) Run "git push". In "Username" put your Github username, and in "Password" use the token that you copied in step b). This should now be reflected in your branch. + +When you are creating a pull request to the `s2025_teamX` branch of GEMstack, you can do this through the Github webpage. You can also use the hub command line tool: https://hub.github.com/. Ensure that this shows up in the `s2025_teamX` branch's Pull Requests tab. + +**Quality-of-life tips** +- By default, the log folder is shown after each run. If this is annoying, change the launch file to read `after: show_log_folder: False`. +- Each component can generate CSV files that collect fast streaming data for later analysis. Use `self.debug(item,value)` or `self.debug_event(label)` in your component's initialize(), update(), or cleanup() functions. These will be stored in LOG_FOLDER/COMPONENT_debug.csv. +- Find all magic constants in your code. Move them into the `GEMstack/knowledge/defaults/current.yaml` file under appropriate sub-keys. + + +**To submit**: ensure your group's work is merged into the `s2025_teamA` or `s2025_teamB` branches (as appropriate) with appropriately logged PRs. We will review your team's branch history. Your group will submit some evidence of its component working, and if successful your team will submit an integrated video of the behavior working. These may be posted to Slack or Clickup. + + +## Group Leads + +- Decide on a good day of the week and time for group meetings. +- Discuss and decide on Github pull request procedures for your team's branches. +- Discuss and decide on project management and communication features for your team (e.g., Slack / Clickup). +- Lead your group and team with clear expectations to prepare work in time for integration. Don't leave this to the last minute! Suggest establishing an internal deadline of 2/17 for all technical pieces. +- Keep track of team members' efforts, especially if their contributions are not apparent through branch commit history. At the end of this phase you will be reporting on your members' contributions, so if they are not listed in PRs and you have little to say about their work, they will receive low individual scores for this assignment. Note that they will be reporting on your leadership as well, so remember to treat your group fairly. +- Gather sufficient evidence (e.g., videos of partial system tests or simulation tests, or a report with text and figures) that your group has succeeded in its objectives. +- Decide which group on your team will test the integrated behavior. Have them report results, and capture a video of successful runs. + + +## Control & Dynamics Group +- Starter code is provided in the `homework/` folder in your group's branch. +- All members must complete safety lookout training; at least two must complete safety driver training. + +Part 1: + +1. Flash a distress signal to the vehicle via ROS. Modify the `blink.py` program to print messages from some of the "/pacmod/parsed_tx/X" topics (at least 2). For each topic, you will need to create a ROS subscriber. Read the documentation on https://github.com/astuff/pacmod2 to figure out which message types to use and extract printable data from these messages. + + Now, create a publisher for the "/pacmod/as_rx/turn_cmd" topic. You will then flash a "distress signal" corresponding to left turn signal for 2 seconds, right turn signal for 2s, and then turn signals off for 2s. This will then repeat forever until you press Ctrl+C. + + To run your code on the vehicle, you will launch the sensors and drive-by-wire system, then enable Pacmod control via the joystick. (Please do not try to operate the vehicle with the joystick!) Then, run your program with `python3 blink.py`. After you are done, you should disable Pacmod control via the joystick. + +2. Adapt `blink.py` to run the behavior in the GEMstack executor. Copy blink_launch.yaml to `GEMstack/launch`. Copy `blink_component.py` to `GEMstack/onboard/planning/` and make your edits there. + + Replicate your sensor printing and distress signal code in the BlinkDistress class in blink_component.py. You will need to adapt your code from using ROS subscribers / publishers to using the `GEMVehicleInterface` and `GEMVehicleReading` objects provided in the template code. Opening the GEMstack folder in the VSCode IDE will help you find the documentation for these classes. + + Run your code in simulation first. (If you are running on the vehicle, first open one terminal window and run `roscore`. Keep this running.) Then, in the GEMstack folder, run `python3 main.py --variant=sim launch/blink_launch.yaml`. You should see your blinking sequence in a matplotlib window. Use Ctrl+C to quit. + + Now, run your code on the real vehicle. Enable Pacmod control as before, then run `python3 main.py launch/blink_launch.yaml`. + +3. Adapt your code so the distress signal runs on GEMstack as a `signaling` component, and blinks the distress signal only when the `AllState.intent.intent=HALTING` flag is set. You will need to modify the computation graph in `GEMstack/knowledge/defaults` to add the `signaling` component, and modify launch files so your class is no longer listed under `trajectory_tracking`. +4. Use Git to create a pull request, and have your PR approved to the `s2025_teamX` branch. + +Part 2: +1. Tune the low-level controller (currently `PurePursuit`) to track the trajectory `AllState.trajectory` produced by the Planning group as accurately as possible. You may tune steering, acceleration/braking, the tracking strategies, or all three. +2. Decide on an appropriate tracking quality metric with the Infra team. +3. Report on what you tuned and how the tracking accuracy metric was improved. +4. Move all magic constants to an appropriate settings file, and have your pull request approved. +5. Integrate as necessary. + + +## State estimation & Calibration Group + +- Have at least one team member complete safety lookout & driver training. + +Part 1: + +1. You will be using both the Oak RGB-D camera and the Ouster top lidar to estimate the 3D positions of pedestrians relative to the vehicle frame. Because the stereo depth estimate is not as accurate as the lidar, we will be using the image to detect objects and the lidar to measure their 3D extent. The first step of this process is to associate points in the camera with points in the lidar, which requires an understanding of projection and relative pose (extrinsics) calibration. +2. You will first need to capture a calibration dataset consisting of paired images and lidar scans. Write code to capture paired scans and take several snapshots of interesting scenes. You should save images as standard image files, and save lidar scans as numpy arrays or PCD files. Store these to a directory named GEMstack/data/SOMETHING where "SOMETHING" is a descriptive name of your dataset. In the main GEMstack folder we had a GEM e2 program `python3 GEMstack/offboard/calibration/capture_lidar_zed.py data/SOMETHING` that could capture these files. Create a similar program `capture_ouster_oak.py` that reads from the appropriate ROS topics. +3. Use the `CameraInfo` message to obtain the intrinsics of the camera (http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/CameraInfo.html) from the `/oak/rgb/camera_info` ROS topic. You may use the image_geometry module (https://docs.ros.org/en/api/image_geometry/html/python/) to project points in the camera frame to the image. A camera frame is defined with the z axis pointing forward, x pointing right, and y pointing down. The important values of this message are the fx, fy, cx ,and cy intrinsic parameter terms. You can store these values to disk or read them live in your code. +4. We do not have the relative transform `T_toplidar^frontcamera` that converts points from the top lidar (Ouster) frame to the Oak frame. Create a way to calibrate this transform. Specifically, if you have a dataset of paired image, point cloud pairs `(image^t,point_cloud^t)`, you should observe that the points belonging to some object in the point cloud are mapped directly to those pixels in the image. Specifically, if `image_pt` is an image pixel belonging to point pt in the point cloud, then we should observe the relation "image_pt = project(T_toplidar^frontcamera * pt, oak_intrinsics)" + + To obtain this matrix you may use manual calibration if you need to, or you could use a more sophisticated method. + + For manual calibration, you can modify the GEM e2 code `python3 GEMstack/offboard/calibration/klampt_show_lidar_zed.py data/SOMETHING` to visualize the stored point clouds from the front camera and the lidar. You can see why the forward depth estimates are quite poor. + + Automated methods could use a detection method to obtain a distinctive shape like a sphere or a square (e.g., a face of the foam cubes) in the Zed image, paired with the same shape in the lidar point cloud. An optimization would ensure that all lidar points belonging to the shape are projected to its projection in the image. You can also match the stereo and lidar point clouds using a dynamically-associated registration method like ICP. + + +Part 2: + +1. Now we will perform absolute calibration of vehicle height, rear axle center, and sensors, i.e., the vehicle frame. The vehicle frame is centered at the rear axle center, with x pointing forward, y pointing to the left, and z pointing up. We want the transforms `T_frontcamera^vehicle` and `T_toplidar^vehicle`. + + - A flat ground and walls are good references for the pitch/roll of the sensors. Using the lidar point cloud, calibrate the rotation component of this transform. Also, use the height of the ground to determine the z above ground. + - Using distinctive shapes (e.g., the foam cubes), create a dataset in which you set up objects on the centerline of the vehicle. Use these to calibrate the yaw and the horizontal offset of the lidar. + - Measure the height of the rear axle over the ground. Use this to figure out the z above the rear axle center. + +2. Combine all of these measurements to establish `T_toplidar^vehicle`. Use your result in Step 1 to compute `T_frontcamera^vehicle` as well. Store these calibrations into the files referenced by `GEMstack/knowledge/calibration/gem_e4.yaml`. Name your calibrations meaningfully -- specifying which vehicle and what type of calibration you are performing -- and include the time and data of the calibration in your files. +3. Put your offline calibration code in the `GEMstack/offboard/calibration/` directory and write a README with a description of the method that you used, and instructions about how to use it. Describe the results of your calibration steps 1 and 2 in this file. + + DO NOT commit data files used in calibration. Put your datasets in the `data` directory, named appropriately. Files in this directory will not be committed to the repo. + +4. Use Git to create a pull request to the `s2025_teamX` branch, and have your PR approved. +5. Integrate as necessary. + + +## Perception Group + +- Starter code is provided in the `homework/` folder in your group's branch. +- Have at least one team member complete safety lookout & driver training. +- Many of your team members should plan to work on the vehicle, at least in the early stages of the assignment. Plan to coordinate with the Infra team to enable a method for working offline. + +Part 1: +1. Use an object detector to identify a pedestrian from the front camera. Implement the function `detect_people(img)` in `person_detector.py` to return a list of bounding boxes of people in an image. This is a function that inputs OpenCV images and returns a list of (x,y,w,h) image boxes that contain people. To run it, call `person_detector.py IMAGE` where IMAGE is an image file. You may download images from the internet or take your own. If you have a webcam, you can run `person_detector.py webcam` to run the live detector. + + I suggest using the YOLOv8 package (https://github.com/ultralytics/ultralytics) and choosing one of its pretrained models, such as `yolov8n.pt`. Follow the basic tutorials for running the detector. The ID of people is 0, and you can examine the Result object to extract out the boxes that have that ID. +2. Refactor your detector to work with GEMstack. Move your detector logic into a GEMstack Component defined in `pedestrian_detection.py`. The provided code will turn your boxes into a dictionary of `AgentState` objects. Read the documentation of the `AgentState` class to see what data should be stored there. For now, you will put temporary values for most of these fields. + + Specifically you should: + + 1. Implement the `PedestrianDetector2D` component to call your detection function in `image_callback(img)`. Your model should be loaded once in the component's `initialize()` method. + 2. Move your pretrained model to `GEMstack/knowledge/detection`. + 3. Move the Python code to `GEMstack/onboard/perception` where the launch file can find them. + 4. Make sure that your code can find the pretrained model in the `knowledge` directory. + 5. Using the detector-only variant of the launch file `python3 main.py --variant=detector_only pedestrian_detection.yaml`, test your code on the real vehicle. You will need to run "source ~/demo_ws/devel/setup.bash" and "roslaunch basic_launch sensor_init.launch" in another terminal, and run the same "source ..." command before running GEMstack code. + 6. Have a group member(s) walk in front of the Zed camera and observe the printouts. + 7. Finally, tune the rate at which you run your component to obtain the highest framerate possible. + +3. Use Git to create a pull request to `s2025_teamX`, and have your PR approved. + +Part 2: +1. Fuse detections with depth (`front_depth` sensor), multiple cameras, and/or LIDAR (`top_lidar` sensor) to reduce errors and place agents in their 3D locations in the scene. Use the sensor calibration from the Calibration group. Look at the lidar readings near the center of the detection, and use those to estimate a region of interest in the point cloud. Then, fit the center and dimensions of the object to the points in the point cloud that you associate with the pedestrian. Finally, put the estimated center and dimensions of the pedestrian into the "pose" and "dimensions" attributes, making sure to respect the vehicle's coordinate convention. +2. If the Infra team has worked quickly enough, you should be able to work offline, replaying from ROS bags. It is a good idea to coordinate with them. +3. Provide your results in a `PedestrianDetector` component and provide this to the Planning group. + +Part 3: +1. Keep track of `AgentState` objects as the vehicle moves and when they disappear out of the field of view. +2. Track pedestrians over time to fill out the "velocity" attribute and to give them relatively consistent names over time. + - number pedestrian IDs as "ped1", "ped2", etc. so that if you detect that a single pedestrian has moved across your field of view, you consistently output "ped1" as the agent's ID. + - keep track of the detected pedestrians on the prior frame to estimate the velocity of each pedestrian. Make sure to store 3D previous detections in the START frame. For each current detection, examine whether the box of the new detection overlaps any prior detection. If so, this begins a "track" detection in which you a) keep the last ID, and b) estimate the pedestrian's velocity in the CURRENT frame. The reported velocity should NOT be relative to the vehicle's velocity. If this is a new pedestrian, increment the pedestrian ID and output zero velocity for this frame. + + If you wish to use more sophisticated trackers, e.g., Hungarian algorithm, velocity prediction, etc, you may do so. + +3. Make sure this works while the vehicle is moving. For example, if the pedestrian is staying still but the vehicle is moving, their reported velocity remains relatively close to 0. Provide these results to the Planning group. +4. Integrate as necessary. + + +## Planning Group + +- Have at least one team member complete safety lookout & driver training. + +Part 1: +1. We will introduce two new components that operate on the output of a `PedestrianDetector2D` / `PedestrianDetector` model produced by the perception team: + + - Logic in the `PedestrianYielder` component in `pedestrian_yield_logic.py`. + - A planner in the `YieldTrajectoryPlanner` component in `longitudinal_planning.py`. + + Move these files to `GEMstack/onboard/planning` where launch files can find them. + +3. For the planning logic, you will implement a braking strategy in `longitudinal_plan_brake()`, and implement a trapezoidal velocity profile strategy in `longitudinal_plan()`. + + You will first do your development using a unit test script, then in simulation. Run `test_longitudinal_planning.py` from the main GEMstack folder to plot the results of your methods. Using the launch file `python3 main.py --variant=fake_sim pedestrian_detection.yaml`, you will receive perfect knowledge about the pedestrian. Test to make sure this produces reasonable behavior in the simulation. It should slow and stop at a safe distance when a pedestrian is detected, and then resume slowly when the pedestrian disappears. +4. Run on the vehicle using `--variant fake_real`. This will use a "fake" detector that will just report pedestrians at some point. Work with the Control and Dynamics team to ensure that reasonable behavior results. +5. Use Git to create a pull request, and have your PR approved. + +Part 2: +1. Use collision detection to determine the distance to collision and use that to determine whether to begin to brake. + + - Determine the lookahead distance that you would need to avoid collision at the current velocity and desired braking deceleration. + - For many steps along the route up to that distance, determine whether the vehicle would be likely to hit (more precisely, get sufficiently close) to the pedestrian. Use a 1m lateral distance and 3m longitudinal distance buffer in your vehicle geometry. + - For the distance-to-collision you determine above, use the longitudinal planner to determine your desired acceleration. + + Determine how quickly you can run your planner. If it is too slow (<5Hz), try using the vehicle-pedestrian distance to dynamically determine your collision checking resolution. + +2. Modify the `PedestrianYielder` so that it selects a subset of pedestrians for which the above calculations should be performed. +3. Adapt the motion planner to slow gently for distant crossing pedestrians to boost rate of progress after the pedestrian crosses the vehicle's route. Use the Perception group's estimated velocity for each pedestrian. +4. Using the `--variant real_real` will run on the real vehicle using trajectory predictions from the Perception group. +5. Integrate as necessary. + + +## Infra Group +- Have at least one team member complete safety lookout & driver training. + +Part 1: +1. Develop a systematic method for saving camera data for the Calibration and Vision groups. Work closely with them to figure out a method to test their methods from logged data. +2. Write documentation for this method, detailing usage and performance issues. +3. Use Git to create a pull request to `s2025_teamX`, and have your PR approved. + +Part 2: +1. Develop a visualizer for rapidly plotting detected pedestrian trajectories from log data (e.g., matplotlib). +2. Work with the Control & Dynamics team to test the integrated system. +3. Develop metrics for pedestrian safety and passenger comfort. Analyze logs from tests. +4. Integrate as necessary. + diff --git a/homework/blink.py b/homework/blink.py new file mode 100644 index 000000000..5f2c78e7c --- /dev/null +++ b/homework/blink.py @@ -0,0 +1,81 @@ +import rospy +from std_msgs.msg import String, Bool, Float32, Float64 +from pacmod_msgs.msg import PositionWithSpeed, PacmodCmd, SystemRptInt, SystemRptFloat, VehicleSpeedRpt + +# For message format, see +# https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/PacmodCmd.msg + +class BlinkDistress: + """Your control loop code should go here. You will use ROS and Pacmod messages + to communicate with drive-by-wire system to control the vehicle's turn signals. + """ + def __init__(self): + # TODO: Initialize your publishers and subscribers here + + # When you create a subscriber to a /pacmod/parsed_tx/X topic, you will need to provide a callback function. + # You will want this callback to be a BlinkDistress method, such as print_X(self, msg). msg will have a + # ROS message type, and you can find out what this is by either reading the documentation or running + # "rostopic info /pacmod/parsed_tx/X" on the command line. + + pass + + def rate(self): + """Requested update frequency, in Hz""" + return 0.5 + + def initialize(self): + """Run first""" + pass + + def cleanup(self): + """Run last""" + pass + + def update(self): + """Run in a loop""" + # TODO: Implement your control loop here + # You will need to publish a PacmodCmd() to /pacmod/as_rx/turn_cmd. Read the documentation to see + # what the data in the message indicates. + pass + + def healthy(self): + """Returns True if the element is in a stable state.""" + return True + + def done(self): + """Return True if you want to exit.""" + return False + + + +def run_ros_loop(node): + """Executes the event loop of a node using ROS. `node` should support + rate(), initialize(), cleanup(), update(), and done(). You should not modify + this code. + """ + #intializes the node. We use disable_signals so that the end() function can be called when Ctrl+C is issued. + #See http://wiki.ros.org/rospy/Overview/Initialization%20and%20Shutdown + rospy.init_node(node.__class__.__name__, disable_signals=True) + + node.initialize() + rate = rospy.Rate(node.rate()) + termination_reason = "undetermined" + try: + while not rospy.is_shutdown() and not node.done() and node.healthy(): + node.update() + rate.sleep() + if node.done(): + termination_reason = "Node done" + if not node.healthy(): + termination_reason = "Node in unhealthy state" + except KeyboardInterrupt: + termination_reason = "Killed by user" + except Exception as e: + termination_reason = "Exception "+str(e) + raise + finally: + node.cleanup() + rospy.signal_shutdown(termination_reason) + +if __name__ == '__main__': + run_ros_loop(BlinkDistress()) \ No newline at end of file diff --git a/homework/blink_component.py b/homework/blink_component.py new file mode 100644 index 000000000..18fb41c6b --- /dev/null +++ b/homework/blink_component.py @@ -0,0 +1,39 @@ +from ..component import Component +from ..interface.gem import GEMInterface,GEMVehicleCommand,GEMVehicleReading + + +class BlinkDistress(Component): + """Your control loop code should go here. You will use GEMVehicleCommand + to communicate with drive-by-wire system to control the vehicle's turn signals. + """ + def __init__(self, vehicle_interface : GEMInterface): + self.vehicle_interface = vehicle_interface + self.command = None + + def rate(self): + """Requested update frequency, in Hz""" + return 0.5 + + def initialize(self): + """Run first""" + pass + + def cleanup(self): + """Run last""" + pass + + def update(self): + """Run in a loop""" + # we need to set up a GEMVehicleCommand which encapsulates all commands that will be + # sent to the drive-by-wire system, simultaneously. To avoid doing arbitrary things + # to the vehicle, let's maintain the current values (e.g., accelerator, brake pedal, + # steering angle) from its current readings. + command = self.vehicle_interface.command_from_reading() + # TODO: alter command to execute turn signals, then uncomment line below to send + # the command to vehicle + # self.vehicle_interface.send_command(command) + + def healthy(self): + """Returns True if the element is in a stable state.""" + return True + diff --git a/homework/blink_launch.yaml b/homework/blink_launch.yaml new file mode 100644 index 000000000..5e97da806 --- /dev/null +++ b/homework/blink_launch.yaml @@ -0,0 +1,58 @@ +description: "Launch just the blink distress code" +mode: hardware +vehicle_interface: gem_hardware.GEMHardwareInterface +mission_execution: StandardExecutor +require_engaged: False +# Recovery behavior after a component failure +recovery: + planning: + trajectory_tracking : blink_component.BlinkDistress +# Driving behavior for the GEM vehicle. Runs perception and planner but doesn't execute anything (no controller). +drive: + planning: + trajectory_tracking : blink_component.BlinkDistress +log: + # Specify the top-level folder to save the log files. Default is 'logs' + folder : 'logs' + # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix + prefix : 'hw1_' + # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix + #suffix : 'test3' + # Specify which ros topics to record to vehicle.bag. + #ros_topics : ['/pacmod/as_rx/turn_cmd','/pacmod/as_tx/turn_rpt'] + # Specify options to pass to rosbag record. Default is no options. + #rosbag_options : '--split --size=1024' + # If True, then record all readings / commands of the vehicle interface. Default False + vehicle_interface : True + # Specify which components to record to behavior.json. Default records nothing + components : [] + # Specify which components of state to record to state.json. Default records nothing + #state: ['all'] + # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate + #state_rate: 10 +replay: # Add items here to set certain topics / inputs to be replayed from logs + # Specify which log folder to replay from + log: + ros_topics : [] + components : [] + +#usually can keep this constant +computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" + +variants: + sim: + run: + description: "Runs the distress signal, but in simulation" + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/xyhead_demo.yaml' + require_engaged: False + visualization: !include "klampt_visualization.yaml" + recovery: + perception: + state_estimation : OmniscientStateEstimator + drive: + perception: + state_estimation : OmniscientStateEstimator diff --git a/homework/longitudinal_planning.py b/homework/longitudinal_planning.py new file mode 100644 index 000000000..c3f3bf737 --- /dev/null +++ b/homework/longitudinal_planning.py @@ -0,0 +1,93 @@ +from typing import List +from ..component import Component +from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, ObjectFrameEnum +from ...utils import serialization +from ...mathutils.transforms import vector_madd + +def longitudinal_plan(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float) -> Trajectory: + """Generates a longitudinal trajectory for a path with a + trapezoidal velocity profile. + + 1. accelerates from current speed toward max speed + 2. travel along max speed + 3. if at any point you can't brake before hitting the end of the path, + decelerate with accel = -deceleration until velocity goes to 0. + """ + path_normalized = path.arc_length_parameterize() + #TODO: actually do something to points and times + points = [p for p in path_normalized.points] + times = [t for t in path_normalized.times] + trajectory = Trajectory(path.frame,points,times) + return trajectory + + +def longitudinal_brake(path : Path, deceleration : float, current_speed : float) -> Trajectory: + """Generates a longitudinal trajectory for braking along a path.""" + path_normalized = path.arc_length_parameterize() + #TODO: actually do something to points and times + points = [p for p in path_normalized.points] + times = [t for t in path_normalized.times] + trajectory = Trajectory(path.frame,points,times) + return trajectory + + +class YieldTrajectoryPlanner(Component): + """Follows the given route. Brakes if you have to yield or + you are at the end of the route, otherwise accelerates to + the desired speed. + """ + def __init__(self): + self.route_progress = None + self.t_last = None + self.acceleration = 0.5 + self.desired_speed = 1.0 + self.deceleration = 2.0 + + def state_inputs(self): + return ['all'] + + def state_outputs(self) -> List[str]: + return ['trajectory'] + + def rate(self): + return 10.0 + + def update(self, state : AllState): + vehicle = state.vehicle # type: VehicleState + route = state.route # type: Route + t = state.t + + if self.t_last is None: + self.t_last = t + dt = t - self.t_last + + curr_x = vehicle.pose.x + curr_y = vehicle.pose.y + curr_v = vehicle.v + + #figure out where we are on the route + if self.route_progress is None: + self.route_progress = 0.0 + closest_dist,closest_parameter = state.route.closest_point_local((curr_x,curr_y),[self.route_progress-5.0,self.route_progress+5.0]) + self.route_progress = closest_parameter + + #extract out a 10m segment of the route + route_with_lookahead = route.trim(closest_parameter,closest_parameter+10.0) + + #parse the relations indicated + should_brake = False + for r in state.relations: + if r.type == EntityRelationEnum.YIELD and r.obj1 == '': + #yielding to something, brake + should_brake = True + should_accelerate = (not should_brake and curr_v < self.desired_speed) + + #choose whether to accelerate, brake, or keep at current velocity + if should_accelerate: + traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v) + elif should_brake: + traj = longitudinal_brake(route_with_lookahead, self.deceleration, curr_v) + else: + traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) + + return traj diff --git a/homework/pedestrian_detection.py b/homework/pedestrian_detection.py new file mode 100644 index 000000000..3134b1e35 --- /dev/null +++ b/homework/pedestrian_detection.py @@ -0,0 +1,84 @@ +from ...state import AllState,VehicleState,ObjectPose,ObjectFrameEnum,AgentState,AgentEnum,AgentActivityEnum +from ..interface.gem import GEMInterface +from ..component import Component +#from ultralytics import YOLO +#import cv2 +from typing import Dict + +def box_to_fake_agent(box): + """Creates a fake agent state from an (x,y,w,h) bounding box. + + The location and size are pretty much meaningless since this is just giving a 2D location. + """ + x,y,w,h = box + pose = ObjectPose(t=0,x=x+w/2,y=y+h/2,z=0,yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT) + dims = (w,h,0) + return AgentState(pose=pose,dimensions=dims,outline=None,type=AgentEnum.PEDESTRIAN,activity=AgentActivityEnum.MOVING,velocity=(0,0,0),yaw_rate=0) + + +class PedestrianDetector2D(Component): + """Detects pedestrians.""" + def __init__(self,vehicle_interface : GEMInterface): + self.vehicle_interface = vehicle_interface + #self.detector = YOLO('../../knowledge/detection/yolov8n.pt') + self.last_person_boxes = [] + + def rate(self): + return 4.0 + + def state_inputs(self): + return ['vehicle'] + + def state_outputs(self): + return ['agents'] + + def initialize(self): + #tell the vehicle to use image_callback whenever 'front_camera' gets a reading, and it expects images of type cv2.Mat + #self.vehicle_interface.subscribe_sensor('front_camera',self.image_callback,cv2.Mat) + pass + + #def image_callback(self, image : cv2.Mat): + # detection_result = self.detector(image) + # self.last_person_boxes = [] + # #uncomment if you want to debug the detector... + # #for bb in self.last_person_boxes: + # # x,y,w,h = bb + # # cv2.rectangle(image, (int(x-w/2), int(y-h/2)), (int(x+w/2), int(y+h/2)), (255, 0, 255), 3) + # #cv2.imwrite("pedestrian_detections.png",image) + + def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: + res = {} + for i,b in enumerate(self.last_person_boxes): + x,y,w,h = b + res['pedestrian'+str(i)] = box_to_fake_agent(b) + if len(res) > 0: + print("Detected",len(res),"pedestrians") + return res + + +class FakePedestrianDetector2D(Component): + """Triggers a pedestrian detection at some random time ranges""" + def __init__(self,vehicle_interface : GEMInterface): + self.vehicle_interface = vehicle_interface + self.times = [(5.0,20.0),(30.0,35.0)] + self.t_start = None + + def rate(self): + return 4.0 + + def state_inputs(self): + return ['vehicle'] + + def state_outputs(self): + return ['agents'] + + def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: + if self.t_start is None: + self.t_start = self.vehicle_interface.time() + t = self.vehicle_interface.time() - self.t_start + res = {} + for times in self.times: + if t >= times[0] and t <= times[1]: + res['pedestrian0'] = box_to_fake_agent((0,0,0,0)) + print("Detected a pedestrian") + return res diff --git a/homework/pedestrian_detection.yaml b/homework/pedestrian_detection.yaml new file mode 100644 index 000000000..0fffa89fc --- /dev/null +++ b/homework/pedestrian_detection.yaml @@ -0,0 +1,97 @@ +description: "Run the yielding trajectory planner on the real vehicle with real perception" +mode: hardware +vehicle_interface: gem_hardware.GEMHardwareInterface +mission_execution: StandardExecutor + +# Recovery behavior after a component failure +recovery: + planning: + trajectory_tracking : recovery.StopTrajectoryTracker + +# Driving behavior for the GEM vehicle. Runs real pedestrian perception, yield planner, but does not send commands to real vehicle. +drive: + perception: + state_estimation : GNSSStateEstimator + agent_detection : pedestrian_detection.PedestrianDetector2D + perception_normalization : StandardPerceptionNormalizer + planning: + relations_estimation: pedestrian_yield_logic.PedestrianYielder + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv','start'] + motion_planning: longitudinal_planning.YieldTrajectoryPlanner + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + print: False + + +log: + # Specify the top-level folder to save the log files. Default is 'logs' + #folder : 'logs' + # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix + #prefix : 'fixed_route_' + # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix + #suffix : 'test3' + # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics. + ros_topics : + # Specify options to pass to rosbag record. Default is no options. + #rosbag_options : '--split --size=1024' + # If True, then record all readings / commands of the vehicle interface. Default False + vehicle_interface : True + # Specify which components to record to behavior.json. Default records nothing + components : ['state_estimation','agent_detection','motion_planning'] + # Specify which components of state to record to state.json. Default records nothing + #state: ['all'] + # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate + #state_rate: 10 +replay: # Add items here to set certain topics / inputs to be replayed from logs + # Specify which log folder to replay from + log: + ros_topics : [] + components : [] + +#usually can keep this constant +computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" + +variants: + detector_only: + run: + description: "Run the pedestrian detection code" + drive: + planning: + trajectory_tracking: + real_sim: + run: + description: "Run the pedestrian detection code with real detection and fake simulation" + mode: hardware + vehicle_interface: + type: gem_mixed.GEMRealSensorsWithSimMotionInterface + args: + scene: !relative_path '../scenes/xyhead_demo.yaml' + mission_execution: StandardExecutor + require_engaged: False + visualization: !include "klampt_visualization.yaml" + drive: + perception: + state_estimation : OmniscientStateEstimator + + + fake_real: + run: + description: "Run the yielding trajectory planner on the real vehicle with faked perception" + drive: + perception: + agent_detection : pedestrian_detection.FakePedestrianDetector2D + + fake_sim: + run: + description: "Run the yielding trajectory planner in simulation with faked perception" + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/xyhead_demo.yaml' + visualization: !include "klampt_visualization.yaml" + drive: + perception: + state_estimation : OmniscientStateEstimator diff --git a/homework/pedestrian_yield_logic.py b/homework/pedestrian_yield_logic.py new file mode 100644 index 000000000..2567c0093 --- /dev/null +++ b/homework/pedestrian_yield_logic.py @@ -0,0 +1,22 @@ +from ...state import AgentState,AgentEnum,EntityRelation,EntityRelationEnum +from ..component import Component +from typing import List,Dict + +class PedestrianYielder(Component): + """Yields for all pedestrians in the scene. + + Result is stored in the relations graph. + """ + def rate(self): + return None + def state_inputs(self): + return ['agents'] + def state_outputs(self): + return ['relations'] + def update(self,agents : Dict[str,AgentState]) -> List[EntityRelation]: + res = [] + for n,a in agents.items(): + if a.type == AgentEnum.PEDESTRIAN: + #relation: ego-vehicle yields to pedestrian + res.append(EntityRelation(type=EntityRelationEnum.YIELDING,obj1='',obj2=n)) + return res diff --git a/homework/person_detector.py b/homework/person_detector.py new file mode 100644 index 000000000..8f4fa3588 --- /dev/null +++ b/homework/person_detector.py @@ -0,0 +1,48 @@ +#from ultralytics import YOLO +import cv2 +import sys + +def person_detector(img : cv2.Mat): + #TODO: implement me to produce a list of (x,y,w,h) bounding boxes of people in the image + return [] + +def main(fn): + image = cv2.imread(fn) + bboxes = person_detector(image) + print("Detected",len(bboxes),"people") + for bb in bboxes: + x,y,w,h = bb + if not isinstance(x,(int,float)) or not isinstance(y,(int,float)) or not isinstance(w,(int,float)) or not isinstance(h,(int,float)): + print("WARNING: make sure to return Python numbers rather than PyTorch Tensors") + print("Corner",(x,y),"size",(w,h)) + cv2.rectangle(image, (int(x-w/2), int(y-h/2)), (int(x+w/2), int(y+h/2)), (255, 0, 255), 3) + cv2.imshow('Results', image) + cv2.waitKey(0) + +def main_webcam(): + cap = cv2.VideoCapture(0) + cap.set(3, 640) + cap.set(4, 480) + + print("Press space to exit") + while True: + _, image = cap.read() + + bboxes = person_detector(image) + for bb in bboxes: + x,y,w,h = bb + cv2.rectangle(image, (int(x-w/2), int(y-h/2)), (int(x+w/2), int(y+h/2)), (255, 0, 255), 3) + + cv2.imshow('Person detection', image) + if cv2.waitKey(1) & 0xFF == ord(' '): + break + + cap.release() + + +if __name__ == '__main__': + fn = sys.argv[1] + if fn != 'webcam': + main(fn) + else: + main_webcam() \ No newline at end of file diff --git a/homework/test_longitudinal_plan.py b/homework/test_longitudinal_plan.py new file mode 100644 index 000000000..400f34fea --- /dev/null +++ b/homework/test_longitudinal_plan.py @@ -0,0 +1,80 @@ +#needed to import GEMstack from top level directory +import sys +import os +sys.path.append(os.getcwd()) + +from GEMstack.state import Path, ObjectFrameEnum +from GEMstack.onboard.planning.longitudinal_planning import longitudinal_plan,longitudinal_brake +import matplotlib.pyplot as plt + +def test_longitudinal_planning(): + test_path = Path(ObjectFrameEnum.START,[(0,0),(1,0),(2,0),(3,0),(4,0),(5,0),(6,0)]) + test_path2 = Path(ObjectFrameEnum.START,[(5,0),(6,1),(7,2),(9,4)]) + + test_traj = longitudinal_brake(test_path, 2.0, 0.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Braking from 0 m/s (should just stay still)") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + test_traj = longitudinal_brake(test_path, 2.0, 2.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Braking from 2 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + test_traj = longitudinal_plan(test_path, 1.0, 2.0, 3.0, 0.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Accelerating from 0 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + + test_traj = longitudinal_plan(test_path, 1.0, 2.0, 3.0, 2.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Accelerating from 2 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + test_traj = longitudinal_plan(test_path, 0.0, 2.0, 3.0, 3.1) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Keeping constant velocity at 3.1 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + test_traj = longitudinal_plan(test_path, 2.0, 2.0, 20.0, 10.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Too little time to stop, starting at 10 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + test_traj = longitudinal_brake(test_path, 2.0, 10.0) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Too little time to stop, braking at 10 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + test_traj = longitudinal_plan(test_path2, 1.0, 2.0, 3.0, 0.0) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Nonuniform planning") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + + +if __name__ == '__main__': + test_longitudinal_planning() From be692a15af1b05c79d35ba6f83a224412837dc68 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sat, 1 Feb 2025 19:44:04 -0600 Subject: [PATCH 2/9] Fix setup issues and add user to the docker image --- .gitignore | 4 ++ GEMstack/utils/klampt_visualization.py | 6 -- README.md | 66 ++++++++++++++++++- requirements.txt | 2 +- run_docker_container.sh | 8 +++ setup/{Dockerfile => Dockerfile.cuda11.8} | 57 +++++++++++++---- setup/Dockerfile.cuda12 | 78 +++++++++++++++++++++++ setup/build_docker_image.sh | 22 +++++++ setup/docker-compose.yaml | 36 +++++++++++ setup/get_nvidia_container.sh | 17 +++-- setup/setup_this_machine.sh | 54 ++++++++++++---- stop_docker_container.sh | 3 + 12 files changed, 312 insertions(+), 41 deletions(-) create mode 100644 run_docker_container.sh rename setup/{Dockerfile => Dockerfile.cuda11.8} (59%) create mode 100644 setup/Dockerfile.cuda12 create mode 100644 setup/build_docker_image.sh create mode 100644 setup/docker-compose.yaml create mode 100644 stop_docker_container.sh diff --git a/.gitignore b/.gitignore index 0c36d682f..4ea5ac549 100644 --- a/.gitignore +++ b/.gitignore @@ -165,3 +165,7 @@ 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/ diff --git a/GEMstack/utils/klampt_visualization.py b/GEMstack/utils/klampt_visualization.py index 0137f829e..5a73fe95d 100644 --- a/GEMstack/utils/klampt_visualization.py +++ b/GEMstack/utils/klampt_visualization.py @@ -290,12 +290,6 @@ def plot_scene(scene : SceneState, ground_truth_vehicle=None, vehicle_model = No def plot(state : AllState, ground_truth_vehicle = None, vehicle_model=None, title=None, show=True): plot_scene(state, ground_truth_vehicle=ground_truth_vehicle, vehicle_model=vehicle_model, title=title, show=show) if state.route is not None: -<<<<<<< HEAD - plot_path("route",state.route,color=(1,0.5,0,1)) - if state.trajectory is not None: - plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=2) -======= plot_path("route",state.route,color=(1,0.5,0,1),width=2) if state.trajectory is not None: plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=3) ->>>>>>> 32c90b400b9e1f01833a3bd9788abdab03565978 diff --git a/README.md b/README.md index 51f26e290..822816aaf 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,7 @@ You should also have the following Python dependencies installed, which you can - matplotlib - opencv-python - torch -- klampt +- klampt==0.9.2 - shapely - dacite - pyyaml @@ -25,11 +25,71 @@ You should also have the following Python dependencies installed, which you can 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 `` 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//project`, you can access it inside the container at `/home//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 diff --git a/requirements.txt b/requirements.txt index 94db8ba43..340489839 100644 --- a/requirements.txt +++ b/requirements.txt @@ -4,6 +4,6 @@ scipy matplotlib torch shapely -klampt +klampt==0.9.2 pyyaml dacite diff --git a/run_docker_container.sh b/run_docker_container.sh new file mode 100644 index 000000000..c987a9016 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/setup/Dockerfile b/setup/Dockerfile.cuda11.8 similarity index 59% rename from setup/Dockerfile rename to setup/Dockerfile.cuda11.8 index b35c3b85d..c5699563f 100644 --- a/setup/Dockerfile +++ b/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 @@ -43,7 +63,16 @@ RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && rosdep install --from- 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 git clone https://github.com/krishauser/GEMstack.git -b s2025_teamB RUN cd GEMstack && pip3 install -r requirements.txt -RUN echo /catkin_ws/devel/setup.sh +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} + +ENTRYPOINT ["/bin/bash"] \ No newline at end of file diff --git a/setup/Dockerfile.cuda12 b/setup/Dockerfile.cuda12 new file mode 100644 index 000000000..ea57000bc --- /dev/null +++ b/setup/Dockerfile.cuda12 @@ -0,0 +1,78 @@ +FROM nvidia/cuda:12.0.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 +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://stereolabs.sfo2.cdn.digitaloceanspaces.com/zedsdk/4.2/ZED_SDK_Ubuntu20_cuda12.1_v4.2.4.zstd.run -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' +RUN wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc +RUN apt-key add ros.asc +RUN apt-get update +RUN DEBIAN_FRONTEND=noninteractive apt-get install -y ros-noetic-desktop +RUN apt-get install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential python3-catkin-tools +RUN rosdep init +RUN rosdep update + +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 + +# 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 + +# install ROS dependencies and packages +RUN cd /catkin_ws/src && git clone https://github.com/krishauser/POLARIS_GEM_e2.git +RUN cd /catkin_ws/src && git clone --recurse-submodules https://github.com/stereolabs/zed-ros-wrapper.git +RUN cd /catkin_ws/src && git clone https://github.com/astuff/pacmod2.git + #for some reason the ibeo messages don't work? +RUN cd /catkin_ws/src && git clone https://github.com/astuff/astuff_sensor_msgs.git && rm -rf astuff_sensor_msgs/ibeo_msgs +RUN cd /catkin_ws/src && git clone https://github.com/ros-perception/radar_msgs.git \ + && cd radar_msgs && git checkout noetic + +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 -b s2025_teamB +RUN cd GEMstack && pip3 install -r 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} + +ENTRYPOINT ["/bin/bash"] \ No newline at end of file diff --git a/setup/build_docker_image.sh b/setup/build_docker_image.sh new file mode 100644 index 000000000..ec94c0f21 --- /dev/null +++ b/setup/build_docker_image.sh @@ -0,0 +1,22 @@ +#!/bin/bash + +echo "Select CUDA version:" +echo "1) CUDA 11.8" +echo "2) CUDA 12+" +read -p "Enter choice [1-2]: " choice + +case $choice in + 1) + DOCKERFILE=Dockerfile.cuda11.8 + ;; + 2) + DOCKERFILE=Dockerfile.cuda12 + ;; + *) + echo "Invalid choice" + exit 1 + ;; +esac + +export DOCKERFILE +UID=$(id -u) GID=$(id -g) docker compose -f setup/docker-compose.yaml build \ No newline at end of file diff --git a/setup/docker-compose.yaml b/setup/docker-compose.yaml new file mode 100644 index 000000000..981cd610c --- /dev/null +++ b/setup/docker-compose.yaml @@ -0,0 +1,36 @@ +version: '3.9' + +services: + ros1-noetic: + image: gem_stack + container_name: gem_stack-container + build: + context: . + dockerfile: ${DOCKERFILE:-Dockerfile.cuda11.8} # Default to cuda11.8 if not specified + args: + USER: ${USER} + USER_UID: ${UID:-1000} # Pass host UID + USER_GID: ${GID:-1000} # Pass host GID + stdin_open: true + tty: true + volumes: + - "/etc/group:/etc/group:ro" + - "/etc/passwd:/etc/passwd:ro" + - "/etc/shadow:/etc/shadow:ro" + - "/etc/sudoers.d:/etc/sudoers.d:ro" + - "~:/home/${USER}/host" + - "/tmp/.X11-unix:/tmp/.X11-unix:rw" + environment: + - DISPLAY=${DISPLAY} + # - LIBGL_ALWAYS_SOFTWARE=1 # Uncomment if you want to use software rendering (No GPU) + network_mode: host + ipc: host + user: "${USER}:${USER}" + # Un-Comment the following lines if you want to use Nvidia GPU + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: all # alternatively, use `count: all` for all GPUs + capabilities: [gpu] diff --git a/setup/get_nvidia_container.sh b/setup/get_nvidia_container.sh index 466b1989e..4f738e2c8 100755 --- a/setup/get_nvidia_container.sh +++ b/setup/get_nvidia_container.sh @@ -1,9 +1,14 @@ #!/bin/bash -curl -s -L https://nvidia.github.io/nvidia-container-runtime/gpgkey | \ - sudo apt-key add - -distribution=$(. /etc/os-release;echo $ID$VERSION_ID) -curl -s -L https://nvidia.github.io/nvidia-container-runtime/$distribution/nvidia-container-runtime.list | \ - sudo tee /etc/apt/sources.list.d/nvidia-container-runtime.list +curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg \ + && curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \ + sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \ + sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list + +sed -i -e '/experimental/ s/^#//g' /etc/apt/sources.list.d/nvidia-container-toolkit.list + sudo apt-get update -sudo apt-get install nvidia-container-runtime \ No newline at end of file +sudo apt-get install -y nvidia-container-toolkit + +sudo nvidia-ctk runtime configure --runtime=docker +sudo systemctl restart docker \ No newline at end of file diff --git a/setup/setup_this_machine.sh b/setup/setup_this_machine.sh index e30b3aa23..c23503023 100755 --- a/setup/setup_this_machine.sh +++ b/setup/setup_this_machine.sh @@ -1,30 +1,62 @@ #!/bin/bash sudo apt update -sudo apt-get install git python3 python3-pip wget zstd +sudo apt-get install -y git python3 python3-pip wget zstd + +if [ ! -f /opt/ros/noetic/setup.bash ]; then + echo "ROS Noetic not found. Installing ROS Noetic..." + sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' + wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc + sudo apt-key add ros.asc + sudo apt update + sudo DEBIAN_FRONTEND=noninteractive apt install -y ros-noetic-desktop + sudo apt install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential python3-catkin-tools + sudo rosdep init + rosdep update +fi source /opt/ros/noetic/setup.bash #install Zed SDK -wget https://download.stereolabs.com/zedsdk/4.0/cu121/ubuntu20 -O ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run -chmod +x ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run -./ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run -- silent +echo "Select CUDA version:" +echo "1) CUDA 11.8" +echo "2) CUDA 12+" +read -p "Enter choice [1-2]: " choice + +case $choice in + 1) + wget https://download.stereolabs.com/zedsdk/4.0/cu118/ubuntu20 -O zed_sdk.run + ;; + 2) + wget https://stereolabs.sfo2.cdn.digitaloceanspaces.com/zedsdk/4.2/ZED_SDK_Ubuntu20_cuda12.1_v4.2.4.zstd.run -O zed_sdk.run + ;; + *) + echo "Invalid choice" + exit 1 + ;; +esac + +chmod +x zed_sdk.run +./zed_sdk.run -- silent #create ROS Catkin workspace -mkdir catkin_ws -mkdir catkin_ws/src +mkdir -p ~/catkin_ws/src + +# Store current working directory +CURRENT_DIR=$(pwd) #install ROS dependencies and packages -cd catkin_ws/src +cd ~/catkin_ws/src git clone https://github.com/krishauser/POLARIS_GEM_e2.git git clone https://github.com/astuff/pacmod2.git -git clone https://github.com/astuff/astuff_sensor_msgs.git -git clone https://github.com/ros-perception/radar_msgs.git -cd radar_msgs; git checkout noetic; cd .. +git clone https://github.com/astuff/astuff_sensor_msgs.git && rm -rf astuff_sensor_msgs/ibeo_msgs +git clone https://github.com/ros-perception/radar_msgs.git && cd radar_msgs; git checkout noetic; cd .. cd .. #back to catkin_ws +rosdep update rosdep install --from-paths src --ignore-src -r -y catkin_make -DCMAKE_BUILD_TYPE=Release source devel/setup.bash -cd .. #back to GEMstack +cd $CURRENT_DIR +cd .. #install GEMstack Python dependencies python3 -m pip install -r requirements.txt \ No newline at end of file diff --git a/stop_docker_container.sh b/stop_docker_container.sh new file mode 100644 index 000000000..1b1bad2c8 --- /dev/null +++ b/stop_docker_container.sh @@ -0,0 +1,3 @@ +#! /bin/bash + +docker compose -f setup/docker-compose.yaml down \ No newline at end of file From 9206116af44009d6acf96155a5059dfeab4e9a18 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sat, 1 Feb 2025 20:03:18 -0600 Subject: [PATCH 3/9] Dummy commit to trigger sonarqube --- .gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 4ea5ac549..5d34da6bb 100644 --- a/.gitignore +++ b/.gitignore @@ -168,4 +168,4 @@ cython_debug/ .vscode/ setup/zed_sdk.run -cuda/ +cuda/ \ No newline at end of file From 0fcdff4936d28b3d45bf1a59385f5df9e09c9be6 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sat, 1 Feb 2025 20:39:14 -0600 Subject: [PATCH 4/9] Fix the requrements.txt to take from the current repo --- setup/Dockerfile.cuda11.8 | 8 +++++--- setup/Dockerfile.cuda12 | 8 +++++--- setup/build_docker_image.sh | 4 ++-- setup/docker-compose.yaml | 4 ++-- 4 files changed, 14 insertions(+), 10 deletions(-) diff --git a/setup/Dockerfile.cuda11.8 b/setup/Dockerfile.cuda11.8 index c5699563f..cb5557ac3 100644 --- a/setup/Dockerfile.cuda11.8 +++ b/setup/Dockerfile.cuda11.8 @@ -62,9 +62,11 @@ 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 -b s2025_teamB -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} diff --git a/setup/Dockerfile.cuda12 b/setup/Dockerfile.cuda12 index ea57000bc..fd36b87b8 100644 --- a/setup/Dockerfile.cuda12 +++ b/setup/Dockerfile.cuda12 @@ -62,9 +62,11 @@ 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 -b s2025_teamB -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} diff --git a/setup/build_docker_image.sh b/setup/build_docker_image.sh index ec94c0f21..0852f27ab 100644 --- a/setup/build_docker_image.sh +++ b/setup/build_docker_image.sh @@ -7,10 +7,10 @@ read -p "Enter choice [1-2]: " choice case $choice in 1) - DOCKERFILE=Dockerfile.cuda11.8 + DOCKERFILE=setup/Dockerfile.cuda11.8 ;; 2) - DOCKERFILE=Dockerfile.cuda12 + DOCKERFILE=setup/Dockerfile.cuda12 ;; *) echo "Invalid choice" diff --git a/setup/docker-compose.yaml b/setup/docker-compose.yaml index 981cd610c..a16cdd06c 100644 --- a/setup/docker-compose.yaml +++ b/setup/docker-compose.yaml @@ -5,8 +5,8 @@ services: image: gem_stack container_name: gem_stack-container build: - context: . - dockerfile: ${DOCKERFILE:-Dockerfile.cuda11.8} # Default to cuda11.8 if not specified + context: .. + dockerfile: ${DOCKERFILE:-setup/Dockerfile.cuda11.8} # Default to cuda11.8 if not specified args: USER: ${USER} USER_UID: ${UID:-1000} # Pass host UID From cff1e29354a99505134d8ad063751a68967bdf0b Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sat, 1 Feb 2025 23:00:41 -0600 Subject: [PATCH 5/9] Update the docker compose service name --- setup/docker-compose.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup/docker-compose.yaml b/setup/docker-compose.yaml index a16cdd06c..e78685225 100644 --- a/setup/docker-compose.yaml +++ b/setup/docker-compose.yaml @@ -1,7 +1,7 @@ version: '3.9' services: - ros1-noetic: + gem-stack-ubuntu-20.04-CUDA: image: gem_stack container_name: gem_stack-container build: From e991f0e17ef0c0f23eb2a40e9e73adfbb508fa38 Mon Sep 17 00:00:00 2001 From: Mohammad Fakhreddine Date: Wed, 12 Feb 2025 14:24:32 -0600 Subject: [PATCH 6/9] feat: added a convert script to convert raw data to rosbags --- .gitignore | 4 +- GEMstack/scripts/__init__.py | 0 GEMstack/scripts/convert_to_rosbag.py | 152 ++++++++++++++++++++++++++ 3 files changed, 155 insertions(+), 1 deletion(-) create mode 100644 GEMstack/scripts/__init__.py create mode 100644 GEMstack/scripts/convert_to_rosbag.py diff --git a/.gitignore b/.gitignore index 5d34da6bb..ade40e5ef 100644 --- a/.gitignore +++ b/.gitignore @@ -168,4 +168,6 @@ cython_debug/ .vscode/ setup/zed_sdk.run -cuda/ \ No newline at end of file +cuda/ +#Ignore ROS bags +*.bag diff --git a/GEMstack/scripts/__init__.py b/GEMstack/scripts/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/GEMstack/scripts/convert_to_rosbag.py b/GEMstack/scripts/convert_to_rosbag.py new file mode 100644 index 000000000..ef812ca73 --- /dev/null +++ b/GEMstack/scripts/convert_to_rosbag.py @@ -0,0 +1,152 @@ +#!/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 # Add this import + +def get_matching_files(png_dir, tif_dir, npz_dir): + """Get files that have corresponding data in all three formats""" + # Get sorted lists of files + png_files = sorted(glob.glob(os.path.join(png_dir, 'color*.png'))) + tif_files = sorted(glob.glob(os.path.join(tif_dir, 'depth*.tif'))) + npz_files = sorted(glob.glob(os.path.join(npz_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( + png_dir, + tif_dir, + npz_dir, + output_bag_path, + frame_interval=0.5 +): + # 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(png_dir, tif_dir, npz_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 * 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' instead of 'points' + + # 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__": + # Define your directories here + png_directory = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/" + tif_directory = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/" + npz_directory = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/" + output_bag = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/output.bag" + + try: + create_rosbag_from_data( + png_directory, + tif_directory, + npz_directory, + output_bag + ) + 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)}") From 5b4b97f624b9b200e5395c6d1cff581cb69ce0ba Mon Sep 17 00:00:00 2001 From: Mohammad Fakhreddine Date: Fri, 14 Feb 2025 11:02:49 -0600 Subject: [PATCH 7/9] Added arg parser --- GEMstack/scripts/convert_to_rosbag.py | 46 +++++++++++++++++---------- 1 file changed, 30 insertions(+), 16 deletions(-) diff --git a/GEMstack/scripts/convert_to_rosbag.py b/GEMstack/scripts/convert_to_rosbag.py index ef812ca73..0c385fc64 100644 --- a/GEMstack/scripts/convert_to_rosbag.py +++ b/GEMstack/scripts/convert_to_rosbag.py @@ -11,14 +11,15 @@ from datetime import datetime import time import rospy -import std_msgs.msg # Add this import +import std_msgs.msg +import argparse -def get_matching_files(png_dir, tif_dir, npz_dir): +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(png_dir, 'color*.png'))) - tif_files = sorted(glob.glob(os.path.join(tif_dir, 'depth*.tif'))) - npz_files = sorted(glob.glob(os.path.join(npz_dir, 'lidar*.npz'))) + 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") @@ -46,9 +47,7 @@ def get_number(filename): return matching_pngs, matching_tifs, matching_npzs def create_rosbag_from_data( - png_dir, - tif_dir, - npz_dir, + dir, output_bag_path, frame_interval=0.5 ): @@ -59,7 +58,7 @@ def create_rosbag_from_data( rospy.init_node('bag_creator', anonymous=True) # Get matching files - png_files, tif_files, npz_files = get_matching_files(png_dir, tif_dir, npz_dir) + png_files, tif_files, npz_files = get_matching_files(dir) print(f"Found {len(png_files)} matching files in all three formats") @@ -128,17 +127,32 @@ def create_rosbag_from_data( 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' + ) + args = parser.parse_args() + directory = args.files_directory + output_bag = args.output_bag # Define your directories here - png_directory = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/" - tif_directory = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/" - npz_directory = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/" - output_bag = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/output.bag" + #png_directory = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/" + #tif_directory = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/" + #npz_directory = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/" + #output_bag = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/output.bag" try: create_rosbag_from_data( - png_directory, - tif_directory, - npz_directory, + directory, output_bag ) print("Successfully created ROS bag file!") From 3708973b32a7b6aecb9166601e6e9d181e8f39ed Mon Sep 17 00:00:00 2001 From: Mohammad Fakhreddine Date: Fri, 14 Feb 2025 11:10:39 -0600 Subject: [PATCH 8/9] Added arg parser to convert_to_rosbags.py script --- GEMstack/scripts/convert_to_rosbag.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/GEMstack/scripts/convert_to_rosbag.py b/GEMstack/scripts/convert_to_rosbag.py index 0c385fc64..d684b7654 100644 --- a/GEMstack/scripts/convert_to_rosbag.py +++ b/GEMstack/scripts/convert_to_rosbag.py @@ -144,11 +144,9 @@ def create_rosbag_from_data( args = parser.parse_args() directory = args.files_directory output_bag = args.output_bag - # Define your directories here - #png_directory = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/" - #tif_directory = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/" - #npz_directory = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/" - #output_bag = "/home/mhmadnour/host/CS588/GEMstack/data/data_sample/data/output.bag" + # 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( From 0255d757ba932c694d6524583b5f09d9603ff6b3 Mon Sep 17 00:00:00 2001 From: Mohammad Fakhreddine Date: Fri, 14 Feb 2025 12:30:23 -0600 Subject: [PATCH 9/9] Edited data acquisition rate to be an input that must be specified --- GEMstack/scripts/convert_to_rosbag.py | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/GEMstack/scripts/convert_to_rosbag.py b/GEMstack/scripts/convert_to_rosbag.py index d684b7654..791ef6e0a 100644 --- a/GEMstack/scripts/convert_to_rosbag.py +++ b/GEMstack/scripts/convert_to_rosbag.py @@ -49,7 +49,7 @@ def get_number(filename): def create_rosbag_from_data( dir, output_bag_path, - frame_interval=0.5 + frame_interval ): # Initialize CV bridge bridge = CvBridge() @@ -73,7 +73,7 @@ def create_rosbag_from_data( 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 * frame_interval) + timestamp = rospy.Time.from_sec(start_time.to_sec() + idx * (1/frame_interval)) try: # Process PNG image @@ -100,7 +100,7 @@ def create_rosbag_from_data( # Process pointcloud NPZ pc_data = np.load(npz_file) - points = pc_data['arr_0'] # Using 'arr_0' instead of 'points' + points = pc_data['arr_0'] # Using 'arr_0' based on the provided files' # Create pointcloud message header = std_msgs.msg.Header() @@ -141,9 +141,15 @@ def create_rosbag_from_data( '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" @@ -151,7 +157,8 @@ def create_rosbag_from_data( try: create_rosbag_from_data( directory, - output_bag + output_bag, + rate ) print("Successfully created ROS bag file!")