From cffde30027dfd778f374bac029bc3fa1449407c0 Mon Sep 17 00:00:00 2001 From: krishauser Date: Mon, 27 Jan 2025 14:32:38 -0500 Subject: [PATCH 01/71] 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 7c35a51a88cfbd84943d9db2e69dbdcaf4f3a5a0 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Fri, 31 Jan 2025 15:26:02 -0600 Subject: [PATCH 02/71] test --- test.test | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 test.test diff --git a/test.test b/test.test new file mode 100644 index 000000000..e69de29bb From bcf2949763f3c184a50570e1d8323dff9546f917 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Fri, 31 Jan 2025 15:28:34 -0600 Subject: [PATCH 03/71] test --- test.test | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 test.test diff --git a/test.test b/test.test deleted file mode 100644 index e69de29bb..000000000 From c76d0583d9265548cb297e9c90a5655a18644ba6 Mon Sep 17 00:00:00 2001 From: animesh Date: Fri, 31 Jan 2025 18:19:56 -0600 Subject: [PATCH 04/71] Example commit for planning team A --- .../onboard/perception}/pedestrian_detection.py | 0 .../onboard/perception}/person_detector.py | 0 .../onboard/planning}/longitudinal_planning.py | 0 .../onboard/planning}/pedestrian_yield_logic.py | 0 GEMstack/utils/klampt_visualization.py | 6 ------ {homework => launch}/pedestrian_detection.yaml | 0 6 files changed, 6 deletions(-) rename {homework => GEMstack/onboard/perception}/pedestrian_detection.py (100%) rename {homework => GEMstack/onboard/perception}/person_detector.py (100%) rename {homework => GEMstack/onboard/planning}/longitudinal_planning.py (100%) rename {homework => GEMstack/onboard/planning}/pedestrian_yield_logic.py (100%) rename {homework => launch}/pedestrian_detection.yaml (100%) diff --git a/homework/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py similarity index 100% rename from homework/pedestrian_detection.py rename to GEMstack/onboard/perception/pedestrian_detection.py diff --git a/homework/person_detector.py b/GEMstack/onboard/perception/person_detector.py similarity index 100% rename from homework/person_detector.py rename to GEMstack/onboard/perception/person_detector.py diff --git a/homework/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py similarity index 100% rename from homework/longitudinal_planning.py rename to GEMstack/onboard/planning/longitudinal_planning.py diff --git a/homework/pedestrian_yield_logic.py b/GEMstack/onboard/planning/pedestrian_yield_logic.py similarity index 100% rename from homework/pedestrian_yield_logic.py rename to GEMstack/onboard/planning/pedestrian_yield_logic.py 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/homework/pedestrian_detection.yaml b/launch/pedestrian_detection.yaml similarity index 100% rename from homework/pedestrian_detection.yaml rename to launch/pedestrian_detection.yaml From be692a15af1b05c79d35ba6f83a224412837dc68 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sat, 1 Feb 2025 19:44:04 -0600 Subject: [PATCH 05/71] 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 06/71] 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 07/71] 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 08/71] 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 2a75f28c018b7750e8adab1ff640cbcb2b9c7d52 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Sun, 2 Feb 2025 15:19:25 -0600 Subject: [PATCH 09/71] script for capturing paird img/points --- .../calibration/capture_ouster_oak.py | 94 +++++++++++++++++++ 1 file changed, 94 insertions(+) create mode 100644 GEMstack/offboard/calibration/capture_ouster_oak.py diff --git a/GEMstack/offboard/calibration/capture_ouster_oak.py b/GEMstack/offboard/calibration/capture_ouster_oak.py new file mode 100644 index 000000000..b8f746199 --- /dev/null +++ b/GEMstack/offboard/calibration/capture_ouster_oak.py @@ -0,0 +1,94 @@ +# ROS Headers +import rospy +from sensor_msgs.msg import Image,PointCloud2 +import sensor_msgs.point_cloud2 as pc2 +import ctypes +import struct + +# OpenCV and cv2 bridge +import cv2 +from cv_bridge import CvBridge +import numpy as np +import os + +lidar_points = None +camera_image = None +depth_image = None +bridge = CvBridge() + +def lidar_callback(lidar : PointCloud2): + global lidar_points + lidar_points = lidar + +def camera_callback(img : Image): + global camera_image + camera_image = img + +def pc2_to_numpy(pc2_msg, want_rgb = False): + gen = pc2.read_points(pc2_msg, skip_nans=True) + if want_rgb: + xyzpack = np.array(list(gen),dtype=np.float32) + if xyzpack.shape[1] != 4: + raise ValueError("PointCloud2 does not have points") + xyzrgb = np.empty((xyzpack.shape[0],6)) + xyzrgb[:,:3] = xyzpack[:,:3] + for i,x in enumerate(xyzpack): + rgb = x[3] + # cast float32 to int so that bitwise operations are possible + s = struct.pack('>f' ,rgb) + i = struct.unpack('>l',s)[0] + # you can get back the float value by the inverse operations + pack = ctypes.c_uint32(i).value + r = (pack & 0x00FF0000)>> 16 + g = (pack & 0x0000FF00)>> 8 + b = (pack & 0x000000FF) + #r,g,b values in the 0-255 range + xyzrgb[i,3:] = (r,g,b) + return xyzrgb + else: + return np.array(list(gen),dtype=np.float32)[:,:3] + +def save_scan(lidar_fn,color_fn): + print("Saving scan to",lidar_fn,color_fn) + pc = pc2_to_numpy(lidar_points,want_rgb=False) + np.savez(lidar_fn,pc) + cv2.imwrite(color_fn,bridge.imgmsg_to_cv2(camera_image)) + +def main(folder='data',start_index=1): + rospy.init_node("capture_lidar_zed",disable_signals=True) + lidar_sub = rospy.Subscriber("/ouster/points", PointCloud2, lidar_callback) + camera_sub = rospy.Subscriber("/oak/rgb/image_raw", Image, camera_callback) + index = start_index + print("Press any key to:") + print(" store lidar point clouds as npz") + print(" store color images as png") + print("Press Escape or Ctrl+C to quit") + while True: + if camera_image: + cv2.imshow("result",bridge.imgmsg_to_cv2(camera_image)) + key = cv2.waitKey(30) + if key == -1: + #nothing + pass + elif key == 27: + #escape + break + else: + if lidar_points is None or camera_image is None or depth_image is None: + print("Missing some messages?") + else: + files = [ + os.path.join(folder,'lidar{}.npz'.format(index)), + os.path.join(folder,'color{}.png'.format(index))] + save_scan(*files) + index += 1 + +if __name__ == '__main__': + import sys + folder = 'data' + start_index = 1 + if len(sys.argv) >= 2: + folder = sys.argv[1] + if len(sys.argv) >= 3: + start_index = int(sys.argv[2]) + main(folder,start_index) From 753e5bad7205d6cfd373f47342abe03ae4f03bac Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sun, 2 Feb 2025 18:23:18 -0600 Subject: [PATCH 10/71] Update missing depedencies and ultralytics python packagein requirements --- requirements.txt | 3 +++ setup/Dockerfile.cuda11.8 | 3 +++ setup/Dockerfile.cuda12 | 3 +++ setup/setup_this_machine.sh | 5 ++++- 4 files changed, 13 insertions(+), 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index 340489839..aba104c78 100644 --- a/requirements.txt +++ b/requirements.txt @@ -7,3 +7,6 @@ shapely klampt==0.9.2 pyyaml dacite + +# Perception +ultralytics diff --git a/setup/Dockerfile.cuda11.8 b/setup/Dockerfile.cuda11.8 index cb5557ac3..687533803 100644 --- a/setup/Dockerfile.cuda11.8 +++ b/setup/Dockerfile.cuda11.8 @@ -68,6 +68,9 @@ COPY requirements.txt /tmp/requirements.txt # Install Python dependencies RUN pip3 install -r /tmp/requirements.txt +# Install other Dependencies +RUN apt-get install -y ros-noetic-septentrio-gnss-driver + USER ${USER} # Add ROS and GEMstack paths to bashrc diff --git a/setup/Dockerfile.cuda12 b/setup/Dockerfile.cuda12 index fd36b87b8..ffafaaa3f 100644 --- a/setup/Dockerfile.cuda12 +++ b/setup/Dockerfile.cuda12 @@ -68,6 +68,9 @@ COPY requirements.txt /tmp/requirements.txt # Install Python dependencies RUN pip3 install -r /tmp/requirements.txt +# Install other Dependencies +RUN apt-get install -y ros-noetic-septentrio-gnss-driver + USER ${USER} # Add ROS and GEMstack paths to bashrc diff --git a/setup/setup_this_machine.sh b/setup/setup_this_machine.sh index c23503023..5c9918866 100755 --- a/setup/setup_this_machine.sh +++ b/setup/setup_this_machine.sh @@ -59,4 +59,7 @@ source devel/setup.bash cd $CURRENT_DIR cd .. #install GEMstack Python dependencies -python3 -m pip install -r requirements.txt \ No newline at end of file +python3 -m pip install -r requirements.txt + +#install other dependencies +sudo apt-get install -y ros-noetic-septentrio-gnss-driver \ No newline at end of file From a7a2e8c2a1b3400a0b579205a5515b32f30c6369 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sun, 2 Feb 2025 18:40:59 -0600 Subject: [PATCH 11/71] Add. Instructions to check if the container toolkit is working as expected --- README.md | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 822816aaf..07342c3b6 100644 --- a/README.md +++ b/README.md @@ -46,7 +46,21 @@ you can install the dependencies or GEMstack by running `setup/setup_this_machin > [!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). +## Prerequisites +- Docker +- Nvidia Container Toolkit + +Try running the sample workload from the [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/sample-workload.html) to check if your system is compatible. + +```bash +sudo docker run --rm --runtime=nvidia --gpus all ubuntu nvidia-smi +``` +You should see the nvidia-smi output similar to [this](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/sample-workload.html#:~:text=all%20ubuntu%20nvidia%2Dsmi-,Your%20output%20should%20resemble%20the%20following%20output%3A,-%2B%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2B%0A%7C%20NVIDIA%2DSMI%20535.86.10). + +If you see the output, you are good to go. Otherwise, you will need to install the Docker and NVidia Container Toolkit by following the instructions. +- For **Docker**, follow the instructions [here](https://docs.docker.com/engine/install/ubuntu/#install-using-the-repository). + +- For **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 From 1f6fb93df0683a8c4b94f642a4524ab16a8d1f7a Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sun, 2 Feb 2025 18:58:50 -0600 Subject: [PATCH 12/71] Update. Readme with latest depedencies --- README.md | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 07342c3b6..e812e663e 100644 --- a/README.md +++ b/README.md @@ -11,17 +11,18 @@ GEMstack uses Python 3.7+ and ROS Noetic. (It is possible to do some offline and simulation work without ROS, but it is highly recommended to install it if you are working on any onboard behavior or training for rosbag files.) You should also have the following Python dependencies installed, which you can install from this folder using `pip install -r requirements.txt`: - -- numpy -- scipy -- matplotlib -- opencv-python -- torch -- klampt==0.9.2 -- shapely -- dacite -- pyyaml - +- GEMstack Dependencies + - numpy + - scipy + - matplotlib + - opencv-python + - torch + - klampt==0.9.2 + - shapely + - dacite + - pyyaml +- Perception Dependencies + - ultralytics 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 6618e8fb5a49dd6b53a43f669fece09871734e39 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sun, 2 Feb 2025 19:36:42 -0600 Subject: [PATCH 13/71] Update. Readme with post install steps --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index e812e663e..463597875 100644 --- a/README.md +++ b/README.md @@ -48,7 +48,7 @@ you can install the dependencies or GEMstack by running `setup/setup_this_machin > Make sure to check the Nvidia Driver and supported CUDA version before proceeding by following the steps in the previous section. ## Prerequisites -- Docker +- Docker (In Linux - Make sure to follow the post-installation steps from [here](https://docs.docker.com/engine/install/linux-postinstall/)) - Nvidia Container Toolkit Try running the sample workload from the [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/sample-workload.html) to check if your system is compatible. From a871397c41f33e1f194af42aff08fe722dcc25d1 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 3 Feb 2025 12:57:07 -0500 Subject: [PATCH 14/71] Fixed launch file to work with standard perception pipeline --- homework/blink_launch.yaml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/homework/blink_launch.yaml b/homework/blink_launch.yaml index 5e97da806..41174f692 100644 --- a/homework/blink_launch.yaml +++ b/homework/blink_launch.yaml @@ -5,10 +5,16 @@ mission_execution: StandardExecutor require_engaged: False # Recovery behavior after a component failure recovery: + perception: + state_estimation : GNSSStateEstimator + perception_normalization : StandardPerceptionNormalizer planning: trajectory_tracking : blink_component.BlinkDistress # Driving behavior for the GEM vehicle. Runs perception and planner but doesn't execute anything (no controller). drive: + perception: + state_estimation : GNSSStateEstimator + perception_normalization : StandardPerceptionNormalizer planning: trajectory_tracking : blink_component.BlinkDistress log: From be315de599389a89c33438af9ff33fd555eb1d5e Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 3 Feb 2025 12:46:58 -0500 Subject: [PATCH 15/71] Fixed diff that was somehow committed --- GEMstack/utils/klampt_visualization.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/GEMstack/utils/klampt_visualization.py b/GEMstack/utils/klampt_visualization.py index 5a73fe95d..063f690c7 100644 --- a/GEMstack/utils/klampt_visualization.py +++ b/GEMstack/utils/klampt_visualization.py @@ -6,6 +6,10 @@ from . import settings from ..state import ObjectFrameEnum,ObjectPose,PhysicalObject,VehicleState,VehicleGearEnum,Path,Obstacle,AgentState,AgentEnum,Roadgraph,RoadgraphLane,RoadgraphLaneEnum,RoadgraphCurve,RoadgraphCurveEnum,RoadgraphRegion,RoadgraphRegionEnum,RoadgraphSurfaceEnum,Trajectory,Route,SceneState,AllState +#KH: there is a bug on some system where the visualization crashes with an OpenGL error when drawing curves +#this is a workaround. We really should find the source of the bug! +MAX_POINTS_IN_CURVE = 50 + OBJECT_COLORS = { AgentEnum.CAR : (1,1,0,1), AgentEnum.PEDESTRIAN : (0,1,0,1), @@ -198,8 +202,8 @@ def plot_vehicle(vehicle : VehicleState, vehicle_model=None, axis_len=1.0): vehicle_model.link('rear_left_stop_light_link').appearance().setColor(0.3,0,0,1) def plot_path(name : str, path : Path, color=(0,0,0), width=1): - if len(path.points) > 50: - vis.add(name,[list(p) for p in path.points[::len(path.points)//50]],color=color,width=width) + if len(path.points) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? + vis.add(name,[list(p) for p in path.points[::len(path.points)//MAX_POINTS_IN_CURVE]],color=color,width=width) else: vis.add(name,[list(p) for p in path.points],color=color,width=width) @@ -214,8 +218,8 @@ def plot_curve(name : str, curve : RoadgraphCurve, color=None, width=None): if width is not None: style['width'] = width for i,seg in enumerate(curve.segments): - if len(seg) > 50: - vis.add(name+"_%d" % i,seg[::len(seg)//50],**style) + if len(seg) > MAX_POINTS_IN_CURVE: # downsample due to OpenGL error? + vis.add(name+"_%d" % i,seg[::len(seg)//MAX_POINTS_IN_CURVE],**style) else: vis.add(name+"_%d" % i,seg,**style) From 6f18feb362170d5df86191bc5b98e7cf9f4873a4 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Mon, 3 Feb 2025 14:19:51 -0600 Subject: [PATCH 16/71] Update volume mounting to access the files --- setup/docker-compose.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/setup/docker-compose.yaml b/setup/docker-compose.yaml index e78685225..8c43cfcaf 100644 --- a/setup/docker-compose.yaml +++ b/setup/docker-compose.yaml @@ -14,10 +14,10 @@ services: 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" + # - "/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: From cabe2fc82201535e28161bc7b3f4a139527baaa3 Mon Sep 17 00:00:00 2001 From: AadarshHegde123 Date: Mon, 3 Feb 2025 23:30:30 -0800 Subject: [PATCH 17/71] updating some ros topics to match gem e4 --- homework/capture_ouster_oak.py | 108 +++++++++++++++++++++++++++++++++ 1 file changed, 108 insertions(+) create mode 100644 homework/capture_ouster_oak.py diff --git a/homework/capture_ouster_oak.py b/homework/capture_ouster_oak.py new file mode 100644 index 000000000..97e245cbb --- /dev/null +++ b/homework/capture_ouster_oak.py @@ -0,0 +1,108 @@ +# ROS Headers +import rospy +from sensor_msgs.msg import Image,PointCloud2 +import sensor_msgs.point_cloud2 as pc2 +import ctypes +import struct + +# OpenCV and cv2 bridge +import cv2 +from cv_bridge import CvBridge +import numpy as np +import os + +lidar_points = None +camera_image = None +depth_image = None +bridge = CvBridge() + +def lidar_callback(lidar : PointCloud2): + global lidar_points + lidar_points = lidar + +def camera_callback(img : Image): + global camera_image + camera_image = img + +def depth_callback(img : Image): + global depth_image + depth_image = img + +def pc2_to_numpy(pc2_msg, want_rgb = False): + gen = pc2.read_points(pc2_msg, skip_nans=True) + if want_rgb: + xyzpack = np.array(list(gen),dtype=np.float32) + if xyzpack.shape[1] != 4: + raise ValueError("PointCloud2 does not have points") + xyzrgb = np.empty((xyzpack.shape[0],6)) + xyzrgb[:,:3] = xyzpack[:,:3] + for i,x in enumerate(xyzpack): + rgb = x[3] + # cast float32 to int so that bitwise operations are possible + s = struct.pack('>f' ,rgb) + i = struct.unpack('>l',s)[0] + # you can get back the float value by the inverse operations + pack = ctypes.c_uint32(i).value + r = (pack & 0x00FF0000)>> 16 + g = (pack & 0x0000FF00)>> 8 + b = (pack & 0x000000FF) + #r,g,b values in the 0-255 range + xyzrgb[i,3:] = (r,g,b) + return xyzrgb + else: + return np.array(list(gen),dtype=np.float32)[:,:3] + +def save_scan(lidar_fn,color_fn,depth_fn): + print("Saving scan to",lidar_fn,color_fn,depth_fn) + pc = pc2_to_numpy(lidar_points,want_rgb=False) + np.savez(lidar_fn,pc) + cv2.imwrite(color_fn,bridge.imgmsg_to_cv2(camera_image)) + dimage = bridge.imgmsg_to_cv2(depth_image) + dimage_non_nan = dimage[np.isfinite(dimage)] + print("Depth range",np.min(dimage_non_nan),np.max(dimage_non_nan)) + dimage = np.nan_to_num(dimage,nan=0,posinf=0,neginf=0) + dimage = (dimage/4000*0xffff) + print("Depth pixel range",np.min(dimage),np.max(dimage)) + dimage = dimage.astype(np.uint16) + cv2.imwrite(depth_fn,dimage) + +def main(folder='data',start_index=1): + rospy.init_node("capture_lidar_zed",disable_signals=True) # what to change to? + lidar_sub = rospy.Subscriber("/ouster/points", PointCloud2, lidar_callback) + camera_sub = rospy.Subscriber("/oak/rgb/image_raw", Image, camera_callback) + depth_sub = rospy.Subscriber("/oak/rgb/image_raw/compressedDepth", Image, depth_callback) + index = start_index + print("Press any key to:") + print(" store lidar point clouds as npz") + print(" store color images as png") + print(" store depth images (m scaled by 0xffff/4000) as 16-bit tif") + print("Press Escape or Ctrl+C to quit") + while True: + if camera_image: + cv2.imshow("result",bridge.imgmsg_to_cv2(camera_image)) + key = cv2.waitKey(30) + if key == -1: + #nothing + pass + elif key == 27: + #escape + break + else: + if lidar_points is None or camera_image is None or depth_image is None: + print("Missing some messages?") + else: + files = [os.path.join(folder,'lidar{}.npz'.format(index)), + os.path.join(folder,'color{}.png'.format(index)), + os.path.join(folder,'depth{}.tif'.format(index))] + save_scan(*files) + index += 1 + +if __name__ == '__main__': + import sys + folder = 'data' + start_index = 1 + if len(sys.argv) >= 2: + folder = sys.argv[1] + if len(sys.argv) >= 3: + start_index = int(sys.argv[2]) + main(folder,start_index) From b39e69036910dccc5ee76dc6513bf5ef7b1a9dd2 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Tue, 4 Feb 2025 13:36:10 -0600 Subject: [PATCH 18/71] Update capture_ouster_oak.py --- homework/capture_ouster_oak.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/homework/capture_ouster_oak.py b/homework/capture_ouster_oak.py index 97e245cbb..6fbf64e17 100644 --- a/homework/capture_ouster_oak.py +++ b/homework/capture_ouster_oak.py @@ -67,7 +67,7 @@ def save_scan(lidar_fn,color_fn,depth_fn): cv2.imwrite(depth_fn,dimage) def main(folder='data',start_index=1): - rospy.init_node("capture_lidar_zed",disable_signals=True) # what to change to? + rospy.init_node("capture_ouster_oak",disable_signals=True) # what to change to? lidar_sub = rospy.Subscriber("/ouster/points", PointCloud2, lidar_callback) camera_sub = rospy.Subscriber("/oak/rgb/image_raw", Image, camera_callback) depth_sub = rospy.Subscriber("/oak/rgb/image_raw/compressedDepth", Image, depth_callback) From 88bdc23825720783ffa4d6294a5b76b6ba8827c2 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Tue, 4 Feb 2025 13:37:01 -0600 Subject: [PATCH 19/71] changed node name for data capturing --- homework/capture_ouster_oak.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/homework/capture_ouster_oak.py b/homework/capture_ouster_oak.py index 6fbf64e17..4e946d931 100644 --- a/homework/capture_ouster_oak.py +++ b/homework/capture_ouster_oak.py @@ -67,7 +67,7 @@ def save_scan(lidar_fn,color_fn,depth_fn): cv2.imwrite(depth_fn,dimage) def main(folder='data',start_index=1): - rospy.init_node("capture_ouster_oak",disable_signals=True) # what to change to? + rospy.init_node("capture_ouster_oak",disable_signals=True) lidar_sub = rospy.Subscriber("/ouster/points", PointCloud2, lidar_callback) camera_sub = rospy.Subscriber("/oak/rgb/image_raw", Image, camera_callback) depth_sub = rospy.Subscriber("/oak/rgb/image_raw/compressedDepth", Image, depth_callback) From 8725e348a3bc9e963628ac4397cb5ee089bcffe5 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Tue, 4 Feb 2025 16:37:22 -0600 Subject: [PATCH 20/71] Update longitudinal_planning.py Add longitudinal_plan and longitudinal_brake --- .../onboard/planning/longitudinal_planning.py | 68 +++++++++++++++++++ 1 file changed, 68 insertions(+) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index c3f3bf737..7dd4daa41 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -14,9 +14,53 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m 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] + + #============================================= + + print("-----LONGITUDINAL PLAN-----") + print("path length: ", path.length()) + length = path.length() + + # Starting point + x0 = points[0][0] + # Time to stop + t_stop = current_speed / deceleration + # Position to stop + x_stop = x0 + current_speed * t_stop - 0.5 * deceleration * t_stop**2 + + # GEM will decelerate to 0 before the end of the path + if length < 10.0 and x_stop > points[-1][0]: + new_points = [] + for t in times: + if t <= t_stop: + x = x0 + current_speed * t - 0.5 * deceleration * t**2 + # Keep the position after reaching 0 velocity + else: + x = x_stop + new_points.append([x, 0]) + points = new_points + print("[BRAKE] Computed points:", points) + + # GEM will accelerate to max speed before the end of the path + else: + new_points = [] + for t in times: + # Accelerate to max speed + if max_speed > current_speed: + x = x0 + current_speed * t + 0.5 * acceleration * t**2 + # Keep the velocity after reaching max speed + else: + x = x0 + current_speed * t + new_points.append([x, 0]) + points = new_points + print("[ACCEL] Computed points:", points) + + #============================================= + trajectory = Trajectory(path.frame,points,times) return trajectory @@ -24,9 +68,33 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m 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] + + #============================================= + + print("=====LONGITUDINAL BRAKE=====") + print("path length: ", path.length()) + length = path.length() + + x0 = points[0][0] + t_stop = current_speed / deceleration + x_stop = x0 + current_speed * t_stop - 0.5 * deceleration * t_stop**2 + + new_points = [] + for t in times: + if t <= t_stop: + x = x0 + current_speed * t - 0.5 * deceleration * t**2 + else: + x = x_stop + new_points.append([x, 0]) + points = new_points + print("[BRAKE] Computed points:", points) + + #============================================= + trajectory = Trajectory(path.frame,points,times) return trajectory From 2b6830ca94cbbd9f932aba21d030c0e9c35c9ac9 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Wed, 5 Feb 2025 11:33:11 -0600 Subject: [PATCH 21/71] Add Yielding planning, just stop in front of pedestrian --- .../onboard/planning/longitudinal_planning.py | 38 ++++++++++++++++++- launch/pedestrian_detection.yaml | 2 + 2 files changed, 38 insertions(+), 2 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 7dd4daa41..8e595b77b 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -4,6 +4,8 @@ from ...utils import serialization from ...mathutils.transforms import vector_madd +import time + 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. @@ -104,6 +106,7 @@ class YieldTrajectoryPlanner(Component): 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 @@ -121,6 +124,8 @@ def rate(self): return 10.0 def update(self, state : AllState): + start_time = time.time() + vehicle = state.vehicle # type: VehicleState route = state.route # type: Route t = state.t @@ -144,10 +149,39 @@ def update(self, state : AllState): #parse the relations indicated should_brake = False + for r in state.relations: - if r.type == EntityRelationEnum.YIELD and r.obj1 == '': + if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': #yielding to something, brake - should_brake = True + + #========================= + """ + Collision detection: + - Compute the lookahead distance required to avoid collision using: + d = v^2/(2*a) + - For many steps along the route (using a resolution that adapts if the + planner runs too slowly), simulate the vehicle's future positions. + - If a pedestrian is detected within 3m longitudinal and 1m lateral buffer, + determine the distance-to-collision. Then compute the required deceleration: + a = -(v^2)/(2*d_collision) + - For distant crossing pedestrians, apply a gentle deceleration based on the + perception-estimated pedestrian velocity. + """ + print("#### YIELDING PLANNING ####") + for n,a in state.agents.items(): + print("ped", a.pose.x,a.pose.y) + print("ego", curr_x,curr_y) + # TEMPORARY: STOP WHEN WITHIN 10M OF PEDESTRIAN + if a.pose.x - curr_x < 10.0 and a.pose.x - curr_x > 0.0: + print("#### Yielding to",n) + should_brake = True + break + + # # UNCOMMENT TO BRAKE FOR ALL PEDESTRIANS + # should_brake = True + + #========================= + should_accelerate = (not should_brake and curr_v < self.desired_speed) #choose whether to accelerate, brake, or keep at current velocity diff --git a/launch/pedestrian_detection.yaml b/launch/pedestrian_detection.yaml index 0fffa89fc..3c5cd9181 100644 --- a/launch/pedestrian_detection.yaml +++ b/launch/pedestrian_detection.yaml @@ -95,3 +95,5 @@ variants: drive: perception: state_estimation : OmniscientStateEstimator + agent_detection : OmniscientAgentDetector + From 63cd8f6a0bf6d12d6d961b5e6c01f1ea35c3ff9a Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Wed, 5 Feb 2025 13:08:34 -0600 Subject: [PATCH 22/71] Fix the poses (align to absolute pose) --- .../onboard/planning/longitudinal_planning.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 8e595b77b..1f5347325 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -134,6 +134,7 @@ def update(self, state : AllState): self.t_last = t dt = t - self.t_last + # Position in vehicle frame (Start (0,0) to (15,0)) curr_x = vehicle.pose.x curr_y = vehicle.pose.y curr_v = vehicle.v @@ -167,14 +168,26 @@ def update(self, state : AllState): - For distant crossing pedestrians, apply a gentle deceleration based on the perception-estimated pedestrian velocity. """ + print("#### YIELDING PLANNING ####") + + # Convert vehicle pose from vehicle to world coordinates + # xyhead_demo: vehicle start at [4, 5], ped walks [15, 2]~[15, 10] + # Is there relative pose of pedestrian or absolute pose of vehicle? + abs_x = vehicle.pose.x + state.start_vehicle_pose.x + abs_y = vehicle.pose.y + state.start_vehicle_pose.y + + # TODO: Check if there are multiple pedestrians for n,a in state.agents.items(): - print("ped", a.pose.x,a.pose.y) - print("ego", curr_x,curr_y) + print("ped", a.pose.x, a.pose.y) + print("ego", abs_x, abs_y) + + # TODO: Make logic for smooth deceleration and re-acceleration # TEMPORARY: STOP WHEN WITHIN 10M OF PEDESTRIAN - if a.pose.x - curr_x < 10.0 and a.pose.x - curr_x > 0.0: + if a.pose.x - abs_x < 10.0 and a.pose.x - abs_x > 0.0: print("#### Yielding to",n) should_brake = True + break # # UNCOMMENT TO BRAKE FOR ALL PEDESTRIANS From e105880f598ff773f74999575033f004db9efe47 Mon Sep 17 00:00:00 2001 From: GEM e4 user Date: Wed, 5 Feb 2025 14:46:35 -0600 Subject: [PATCH 23/71] lidar front camera data capture working, camera info intrinsic capture working --- GEMstack/offboard/calibration/camera_info.py | 104 ++++++++++++++++++ .../calibration/capture_ouster_oak.py | 37 ++++--- 2 files changed, 127 insertions(+), 14 deletions(-) create mode 100644 GEMstack/offboard/calibration/camera_info.py diff --git a/GEMstack/offboard/calibration/camera_info.py b/GEMstack/offboard/calibration/camera_info.py new file mode 100644 index 000000000..3b45110a5 --- /dev/null +++ b/GEMstack/offboard/calibration/camera_info.py @@ -0,0 +1,104 @@ +# ROS Headers +import rospy +from sensor_msgs.msg import Image,PointCloud2, CameraInfo +import sensor_msgs.point_cloud2 as pc2 +import ctypes +import struct +import pickle +import image_geometry + + +# OpenCV and cv2 bridge +import cv2 +from cv_bridge import CvBridge +import numpy as np +import os +import time + +lidar_points = None +camera_image = None +depth_image = None +bridge = CvBridge() + +def lidar_callback(lidar : PointCloud2): + global lidar_points + lidar_points = lidar + +def camera_callback(info : CameraInfo): + global camera_image + camera_image = info + +def pc2_to_numpy(pc2_msg, want_rgb = False): + gen = pc2.read_points(pc2_msg, skip_nans=True) + if want_rgb: + xyzpack = np.array(list(gen),dtype=np.float32) + if xyzpack.shape[1] != 4: + raise ValueError("PointCloud2 does not have points") + xyzrgb = np.empty((xyzpack.shape[0],6)) + xyzrgb[:,:3] = xyzpack[:,:3] + for i,x in enumerate(xyzpack): + rgb = x[3] + # cast float32 to int so that bitwise operations are possible + s = struct.pack('>f' ,rgb) + i = struct.unpack('>l',s)[0] + # you can get back the float value by the inverse operations + pack = ctypes.c_uint32(i).value + r = (pack & 0x00FF0000)>> 16 + g = (pack & 0x0000FF00)>> 8 + b = (pack & 0x000000FF) + #r,g,b values in the 0-255 range + xyzrgb[i,3:] = (r,g,b) + return xyzrgb + else: + return np.array(list(gen),dtype=np.float32)[:,:3] + +def save_scan(cam_path): + model = image_geometry.PinholeCameraModel() + model.fromCameraInfo(camera_image) + print(model.intrinsicMatrix()) + # with open(cam_path, "w") as file: + # pickle.dump(, file) + # print("Saving scan to", cam_path) + +def main(folder='data',start_index=1): + rospy.init_node("capture_cam_info",disable_signals=True) + caminfo_sub = rospy.Subscriber("/oak/rgb/camera_info", CameraInfo, camera_callback) + index = start_index + print("Press any key to:") + print(" store camera info ") + print("Press Escape or Ctrl+C to quit") + while True: + if camera_image: + time.sleep(1) + # key = cv2.waitKey(0) + # if key == -1: + # #nothing + # pass + # elif key == 27: + # #escape + # break + # else: + # print("this is what we want") + # if lidar_points is None or camera_image is None: + # print("Missing some messages?") + # else: + # files = [ + # os.path.join(folder,'lidar{}.npz'.format(index)), + # os.path.join(folder,'color{}.png'.format(index))] + # save_scan(*files) + # index += 1 + + files = [ + os.path.join(folder,'cam_info{}.txt'.format(index))] + save_scan(*files) + index += 1 + +if __name__ == '__main__': + import sys + folder = 'data' + start_index = 1 + if len(sys.argv) >= 2: + folder = sys.argv[1] + if len(sys.argv) >= 3: + start_index = int(sys.argv[2]) + main(folder,start_index) diff --git a/GEMstack/offboard/calibration/capture_ouster_oak.py b/GEMstack/offboard/calibration/capture_ouster_oak.py index b8f746199..a246a1ff0 100644 --- a/GEMstack/offboard/calibration/capture_ouster_oak.py +++ b/GEMstack/offboard/calibration/capture_ouster_oak.py @@ -10,6 +10,7 @@ from cv_bridge import CvBridge import numpy as np import os +import time lidar_points = None camera_image = None @@ -66,22 +67,30 @@ def main(folder='data',start_index=1): while True: if camera_image: cv2.imshow("result",bridge.imgmsg_to_cv2(camera_image)) - key = cv2.waitKey(30) - if key == -1: - #nothing - pass - elif key == 27: - #escape - break - else: - if lidar_points is None or camera_image is None or depth_image is None: - print("Missing some messages?") - else: - files = [ + time.sleep(1) + # key = cv2.waitKey(0) + # if key == -1: + # #nothing + # pass + # elif key == 27: + # #escape + # break + # else: + # print("this is what we want") + # if lidar_points is None or camera_image is None: + # print("Missing some messages?") + # else: + # files = [ + # os.path.join(folder,'lidar{}.npz'.format(index)), + # os.path.join(folder,'color{}.png'.format(index))] + # save_scan(*files) + # index += 1 + + files = [ os.path.join(folder,'lidar{}.npz'.format(index)), os.path.join(folder,'color{}.png'.format(index))] - save_scan(*files) - index += 1 + save_scan(*files) + index += 1 if __name__ == '__main__': import sys From 96a315e97832c29f761dc52dbac85e7bbce32048 Mon Sep 17 00:00:00 2001 From: krishauser Date: Thu, 6 Feb 2025 17:31:12 -0500 Subject: [PATCH 24/71] Fixed fake_fake simulation calls and rename from YIELD -> YIELDING --- homework/longitudinal_planning.py | 2 +- homework/pedestrian_detection.yaml | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/homework/longitudinal_planning.py b/homework/longitudinal_planning.py index c3f3bf737..e862239fe 100644 --- a/homework/longitudinal_planning.py +++ b/homework/longitudinal_planning.py @@ -77,7 +77,7 @@ def update(self, state : AllState): #parse the relations indicated should_brake = False for r in state.relations: - if r.type == EntityRelationEnum.YIELD and r.obj1 == '': + if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': #yielding to something, brake should_brake = True should_accelerate = (not should_brake and curr_v < self.desired_speed) diff --git a/homework/pedestrian_detection.yaml b/homework/pedestrian_detection.yaml index 0fffa89fc..1a00fa797 100644 --- a/homework/pedestrian_detection.yaml +++ b/homework/pedestrian_detection.yaml @@ -94,4 +94,6 @@ variants: visualization: !include "klampt_visualization.yaml" drive: perception: + agent_detection : pedestrian_detection.FakePedestrianDetector2D + #agent_detection : OmniscientAgentDetector #this option reads agents from the simulator state_estimation : OmniscientStateEstimator From 7659c5d783cfed77c7dde340e89593ecc4582ded Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 6 Feb 2025 17:06:53 -0600 Subject: [PATCH 25/71] Longitudinal planning setup where we the points are treated as way points. This means we are concerned with calculating the times associated with when we would arrive at said waypoints. Moreover, I added to the trajectory critical points when we reach top speed and start decelerating. --- .../onboard/planning/longitudinal_planning.py | 173 +++++++++++++++--- 1 file changed, 144 insertions(+), 29 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 1f5347325..19e78bd01 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -20,52 +20,167 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m #TODO: actually do something to points and times points = [p for p in path_normalized.points] times = [t for t in path_normalized.times] - #============================================= print("-----LONGITUDINAL PLAN-----") print("path length: ", path.length()) length = path.length() + # This assumes that the time denomination cannot be changed + # Starting point x0 = points[0][0] - # Time to stop - t_stop = current_speed / deceleration - # Position to stop - x_stop = x0 + current_speed * t_stop - 0.5 * deceleration * t_stop**2 + cur_point = points[0] + cur_time = times[0] + cur_index = 0 - # GEM will decelerate to 0 before the end of the path - if length < 10.0 and x_stop > points[-1][0]: - new_points = [] - for t in times: - if t <= t_stop: - x = x0 + current_speed * t - 0.5 * deceleration * t**2 - # Keep the position after reaching 0 velocity + new_points = [] + new_times = [] + while current_speed > 0 or cur_index == 0: + # we want to iterate through all the points and add them + # to the new points. However, we also want to add "critical points" + # where we reach top speed, begin decelerating, and stop + new_points.append(cur_point) + new_times.append(cur_time) + print("=====================================") + print("new points: ", new_points) + print("current index: ", cur_index) + print("current speed: ", current_speed) + + # Calculate how much time it would take to stop + min_delta_t_stop = current_speed/deceleration + min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 + assert min_delta_x_stop >= 0 + + # Information we will need + #assert cur_index < len(points)-1 + + # Check if we are done + + # If we cannot stop before or stop exactly at the final position requested + if cur_point[0] + min_delta_x_stop >= points[-1][0]: + print("In case one") + # put on the breaks + + # Calculate the next point in a special manner because of too-little time to stop + if cur_index == len(points)-1: + # the next point in this instance would be when we stop + next_point = (cur_point[0] + min_delta_x_stop, 0) else: - x = x_stop - new_points.append([x, 0]) - points = new_points - print("[BRAKE] Computed points:", points) - - # GEM will accelerate to max speed before the end of the path - else: - new_points = [] - for t in times: - # Accelerate to max speed - if max_speed > current_speed: - x = x0 + current_speed * t + 0.5 * acceleration * t**2 - # Keep the velocity after reaching max speed + next_point = points[cur_index+1] + + # do we just progress and update time + if next_point[0] <= points[-1][0]: + print("continuing to next point") + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = next_point + current_speed -= deceleration*delta_t_to_next_x + cur_index += 1 else: - x = x0 + current_speed * t - new_points.append([x, 0]) - points = new_points - print("[ACCEL] Computed points:", points) + # continue to the point in which we would stop + # update to the next point + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_point = next_point + cur_time += delta_t_to_next_x + #current_speed -= delta_t_to_next_x*deceleration + current_speed = 0 + assert current_speed == 0 + + elif current_speed < max_speed: + print("In case two") + next_point = points[cur_index+1] + # accelerate to max speed + delta_t_to_max_speed = (max_speed - current_speed)/acceleration + delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 + + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + print("go to next point") + # accelerate to max speed + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed += delta_t_to_next_x*deceleration + cur_index += 1 + + else: + print("adding new point") + # we would need to add a new point at max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + elif current_speed == max_speed: + next_point = points[cur_index+1] + # continue on with max speed + print("In case three") + + # add point to start decelerating + if next_point[0] + min_delta_x_stop >= points[-1][0]: + print("Adding new point to start decelerating") + cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed + cur_point = [points[-1][0] - min_delta_x_stop,0] + current_speed = max_speed + else: + print("Continuing on to next point") + cur_time += (next_point[0] - cur_point[0])/current_speed + cur_point = next_point + cur_index += 1 + + elif current_speed > max_speed: + next_point = points[cur_index+1] + print("In case four") + # slow down to max speed + delta_t_to_max_speed = (current_speed - max_speed)/deceleration + delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 + + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + # next position will take care of this + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = next_point[0] + current_speed -= delta_t_to_next_x*deceleration + cur_index += 1 + else: + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + else: + # not sure what falls here + raise ValueError("Not sure what to do here") + + new_points.append(cur_point) + new_times.append(cur_time) + + points = new_points + times = new_times + print("[PLAN] Computed points:", points) + print("[TIME] Computed time:", times) #============================================= trajectory = Trajectory(path.frame,points,times) return trajectory +def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: + """Computes the time to go from x0 to x1 with initial velocity v0 and final velocity v1 + with constant acceleration a.""" + # x1 = x0 + v0*t + 0.5*a*t^2 + # x1 - x0 = v0*t + 0.5*a*t^2 + # 0.5*a*t^2 + v0*t + x0 - x1 = 0 + # t = (-v0 + sqrt(v0^2 - 4*0.5*a*(x0-x1)))/(2*0.5*a) + # t = (-v0 + sqrt(v0^2 + 2*a*(x1-x0)))/a + print("x0: ", x0) + print("x1: ", x1) + print("v: ", v) + print("a: ", a) + + t1 = (-v + (v**2 - 2*a*(x0-x1))**0.5)/a + t2 = (-v - (v**2 - 2*a*(x0-x1))**0.5)/a + print("t1: ", t1) + print("t2: ", t2) + + return min(n for n in [t1, t2] if n>0) def longitudinal_brake(path : Path, deceleration : float, current_speed : float) -> Trajectory: """Generates a longitudinal trajectory for braking along a path.""" From 9aacb102b9467efa14ef47449217828c13b85f22 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Thu, 6 Feb 2025 20:08:27 -0600 Subject: [PATCH 26/71] Fixed Nan error, modified Brake mode --- GEMstack/onboard/planning/longitudinal_planning.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 19e78bd01..44c0201b9 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -5,6 +5,7 @@ from ...mathutils.transforms import vector_madd import time +import math 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 @@ -138,7 +139,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m # next position will take care of this delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) cur_time += delta_t_to_next_x - cur_point = next_point[0] + cur_point = [next_point[0], 0] current_speed -= delta_t_to_next_x*deceleration cur_index += 1 else: @@ -177,6 +178,9 @@ def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: t1 = (-v + (v**2 - 2*a*(x0-x1))**0.5)/a t2 = (-v - (v**2 - 2*a*(x0-x1))**0.5)/a + if math.isnan(t1): t1 = 0 + if math.isnan(t2): t2 = 0 + print("t1: ", t1) print("t2: ", t2) @@ -308,6 +312,9 @@ def update(self, state : AllState): # # UNCOMMENT TO BRAKE FOR ALL PEDESTRIANS # should_brake = True + # # UNCOMMENT NOT TO BRAKE + should_brake = False + #========================= should_accelerate = (not should_brake and curr_v < self.desired_speed) From 8059c53e586c671a4d017bf18f427e7a5dea0b7b Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Thu, 6 Feb 2025 22:27:58 -0600 Subject: [PATCH 27/71] Fix bug causing crash at end of sim --- GEMstack/onboard/planning/longitudinal_planning.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 44c0201b9..38abe4490 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -27,6 +27,10 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m print("path length: ", path.length()) length = path.length() + # If the path is too short, just return the path for preventing sudden halt of simulation + if length < 0.05: + return Trajectory(path.frame, points, times) + # This assumes that the time denomination cannot be changed # Starting point @@ -184,7 +188,11 @@ def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: print("t1: ", t1) print("t2: ", t2) - return min(n for n in [t1, t2] if n>0) + valid_times = [n for n in [t1, t2] if n > 0] + if valid_times: + return min(valid_times) + else: + return 0.0 def longitudinal_brake(path : Path, deceleration : float, current_speed : float) -> Trajectory: """Generates a longitudinal trajectory for braking along a path.""" From 67348fe79d6c91ec0d912b8fadfbd80555770580 Mon Sep 17 00:00:00 2001 From: Simon Date: Fri, 7 Feb 2025 15:31:43 -0600 Subject: [PATCH 28/71] More/better comments --- .../onboard/planning/longitudinal_planning.py | 41 +++++++++++++++---- 1 file changed, 33 insertions(+), 8 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 19e78bd01..11d9f2be4 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -47,13 +47,13 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m print("current index: ", cur_index) print("current speed: ", current_speed) - # Calculate how much time it would take to stop + # Information we will need: + # Calculate how much time it would take to stop + # Calculate how much distance it would take to stop min_delta_t_stop = current_speed/deceleration min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 assert min_delta_x_stop >= 0 - # Information we will need - #assert cur_index < len(points)-1 # Check if we are done @@ -69,7 +69,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m else: next_point = points[cur_index+1] - # do we just progress and update time + # keep breaking until the next milestone in path if next_point[0] <= points[-1][0]: print("continuing to next point") delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) @@ -78,22 +78,32 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m current_speed -= deceleration*delta_t_to_next_x cur_index += 1 else: - # continue to the point in which we would stop + # continue to the point in which we would stop (current_velocity = 0) # update to the next point delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) cur_point = next_point cur_time += delta_t_to_next_x + # current_speed would not be exactly zero error would be less than 1e-4 but perfer to just set to zero #current_speed -= delta_t_to_next_x*deceleration current_speed = 0 assert current_speed == 0 + # This is the case where we are accelerating to max speed + # because the first if-statement covers for when we decelerating, + # the only time current_speed < max_speed is when we are accelerating elif current_speed < max_speed: print("In case two") + # next point next_point = points[cur_index+1] # accelerate to max speed + + # calculate the time it would take to reach max speed delta_t_to_max_speed = (max_speed - current_speed)/acceleration + # calculate the distance it would take to reach max speed delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 + # if we would reach max speed after the next point, + # just move to the next point and update the current speed and time if cur_point[0] + delta_x_to_max_speed >= next_point[0]: print("go to next point") # accelerate to max speed @@ -103,6 +113,8 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m current_speed += delta_t_to_next_x*deceleration cur_index += 1 + # this is the case where we would reach max speed before the next point + # we need to create a new point where we would reach max speed else: print("adding new point") # we would need to add a new point at max speed @@ -110,6 +122,10 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m cur_point = [cur_point[0] + delta_x_to_max_speed, 0] current_speed = max_speed + # This is the case where we are at max speed + # special functionality is that this block must + # add a point where we would need to start declerating to reach + # the final point elif current_speed == max_speed: next_point = points[cur_index+1] # continue on with max speed @@ -122,33 +138,42 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m cur_point = [points[-1][0] - min_delta_x_stop,0] current_speed = max_speed else: + # Continue on to next point print("Continuing on to next point") cur_time += (next_point[0] - cur_point[0])/current_speed cur_point = next_point cur_index += 1 - + + # This is an edge case and should only be reach + # if the initial speed is greater than the max speed elif current_speed > max_speed: + # We need to hit the breaks + next_point = points[cur_index+1] print("In case four") # slow down to max speed delta_t_to_max_speed = (current_speed - max_speed)/deceleration delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 + # If we would reach the next point before slowing down to max speed + # keep going until we reach the next point if cur_point[0] + delta_x_to_max_speed >= next_point[0]: - # next position will take care of this delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) cur_time += delta_t_to_next_x cur_point = next_point[0] current_speed -= delta_t_to_next_x*deceleration cur_index += 1 else: + # We would reach max speed before the next point + # we need to add a new point at the point where we + # would reach max speed cur_time += delta_t_to_max_speed cur_point = [cur_point[0] + delta_x_to_max_speed, 0] current_speed = max_speed else: # not sure what falls here - raise ValueError("Not sure what to do here") + raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") new_points.append(cur_point) new_times.append(cur_time) From f91796b9dcfc34bb0e0601d71f781fdb84914096 Mon Sep 17 00:00:00 2001 From: Sai Rohit Muralikrishnan Date: Fri, 7 Feb 2025 18:04:22 -0600 Subject: [PATCH 29/71] Part1 based on V_max and Triangle profile --- .../perception/pedestrian_detection.py | 84 +++++++++ .../onboard/perception/person_detector.py | 48 +++++ .../onboard/planning/longitudinal_planning.py | 172 ++++++++++++++++++ .../planning/pedestrian_yield_logic.py | 22 +++ launch/pedestrian_detection.yaml | 97 ++++++++++ setup/docker-compose.yaml | 16 +- test_longitudinal_plan.py | 80 ++++++++ 7 files changed, 515 insertions(+), 4 deletions(-) create mode 100644 GEMstack/onboard/perception/pedestrian_detection.py create mode 100644 GEMstack/onboard/perception/person_detector.py create mode 100644 GEMstack/onboard/planning/longitudinal_planning.py create mode 100644 GEMstack/onboard/planning/pedestrian_yield_logic.py create mode 100644 launch/pedestrian_detection.yaml create mode 100644 test_longitudinal_plan.py diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py new file mode 100644 index 000000000..3134b1e35 --- /dev/null +++ b/GEMstack/onboard/perception/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/GEMstack/onboard/perception/person_detector.py b/GEMstack/onboard/perception/person_detector.py new file mode 100644 index 000000000..409dd0e4b --- /dev/null +++ b/GEMstack/onboard/perception/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/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py new file mode 100644 index 000000000..68f04e92a --- /dev/null +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -0,0 +1,172 @@ +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 + +import time +import math +import numpy as np + + + +def solve_for_v_peak(v0: float, acceleration: float, deceleration: float, total_length: float) -> float: + + if acceleration <= 0 or deceleration <= 0: + raise ValueError("Acceleration and deceleration cant be negative") + + #Formuala: (v_peak^2 - v0^2)/(2*a) + v_peak^2/(2*d) = total_length + numerator = deceleration * v0**2 + 2 * acceleration * deceleration * total_length + denominator = acceleration + deceleration + v_peak_sq = numerator / denominator + + if v_peak_sq < 0: + return 0.0 + + return math.sqrt(v_peak_sq) + + +def longitudinal_plan(path, acceleration: float, deceleration: float, max_speed: float, current_speed: float): + + + # 1 parametrizatiom. + path_norm = path.arc_length_parameterize(speed=1.0) + total_length = path.length() + + # 2. Compute distances for d_accel,d_decel + if max_speed > current_speed: + d_accel = (max_speed**2 - current_speed**2) / (2 * acceleration) + else: + d_accel = 0.0 # Already at or above max_speed + + d_decel = (max_speed**2) / (2 * deceleration) + + # 3. trapezoidal or triangle? + if d_accel + d_decel <= total_length: + t_accel = (max_speed - current_speed) / acceleration if max_speed > current_speed else 0.0 + t_decel = max_speed / deceleration + d_cruise = total_length - d_accel - d_decel + t_cruise = d_cruise / max_speed if max_speed != 0 else 0.0 + t_final = t_accel + t_cruise + t_decel + profile_type = "trapezoidal" + else: + # Triangular profile: not enough distance to reach max_speed so we will calculate peak speed. + peak_speed = solve_for_v_peak(current_speed, acceleration, deceleration, total_length) + # choose the min just in case + peak_speed = min(peak_speed, max_speed) + t_accel = (peak_speed - current_speed) / acceleration if peak_speed > current_speed else 0.0 + t_decel = peak_speed / deceleration + t_final = t_accel + t_decel + profile_type = "triangular" + + # 4. Create a time grid. + dt = 0.1 # adjust based on computation + times = np.arange(0, t_final + dt, dt) + + # 5. Compute the distance s(t) for each time step. + s_vals = [] + for t in times: + if profile_type == "trapezoidal": + if t < t_accel: + # Acceleration phase. + s = current_speed * t + 0.5 * acceleration * t**2 + elif t < t_accel + t_cruise: + # Cruise phase. + s = d_accel + max_speed * (t - t_accel) + else: + # Deceleration phase. + t_decel_phase = t - (t_accel + t_cruise) + # Compute the remaining distance using the deceleration equation. + s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase)**2 + else: # Triangular profile. + if t < t_accel: + # Acceleration phase. + s = current_speed * t + 0.5 * acceleration * t**2 + else: + t_decel_phase = t - t_accel + s_accel = current_speed * t_accel + 0.5 * acceleration * t_accel**2 + s = s_accel + peak_speed * t_decel_phase - 0.5 * deceleration * t_decel_phase**2 + + # should not exceed total path length + s_vals.append(min(s, total_length)) + + + points = [path_norm.eval(s) for s in s_vals] + + trajectory = Trajectory(path_norm.frame, points, list(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.YIELDING 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 \ No newline at end of file diff --git a/GEMstack/onboard/planning/pedestrian_yield_logic.py b/GEMstack/onboard/planning/pedestrian_yield_logic.py new file mode 100644 index 000000000..2567c0093 --- /dev/null +++ b/GEMstack/onboard/planning/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/launch/pedestrian_detection.yaml b/launch/pedestrian_detection.yaml new file mode 100644 index 000000000..0fffa89fc --- /dev/null +++ b/launch/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/setup/docker-compose.yaml b/setup/docker-compose.yaml index 8c43cfcaf..d0b745c9e 100644 --- a/setup/docker-compose.yaml +++ b/setup/docker-compose.yaml @@ -14,19 +14,27 @@ services: 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" + - "/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" + - "/run/user/${UID}/bus:/run/user/${UID}/bus:ro" + - "/tmp/runtime-${USER}:/tmp/runtime-${USER}:rw" environment: - DISPLAY=${DISPLAY} + - XDG_RUNTIME_DIR=/tmp/runtime-${USER} + - DBUS_SYSTEM_BUS_ADDRESS=unix:path=/var/run/dbus/system_bus_socket + - DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/${UID}/bus + - NVIDIA_DRIVER_CAPABILITIES=all + - NVIDIA_VISIBLE_DEVICES=all # - 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 + runtime: nvidia deploy: resources: reservations: diff --git a/test_longitudinal_plan.py b/test_longitudinal_plan.py new file mode 100644 index 000000000..400f34fea --- /dev/null +++ b/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 15191c725524818420a4f1a937eb320cb4121607 Mon Sep 17 00:00:00 2001 From: AadarshHegde123 Date: Fri, 7 Feb 2025 21:52:04 -0800 Subject: [PATCH 30/71] general code clean up + adding depth subscriber + creating klampt visualizer file for manual calibration --- GEMstack/offboard/calibration/camera_info.py | 67 +------ .../calibration/capture_ouster_oak.py | 51 +++--- .../calibration/klampt_lidar_ouster_show.py | 173 ++++++++++++++++++ 3 files changed, 200 insertions(+), 91 deletions(-) create mode 100644 GEMstack/offboard/calibration/klampt_lidar_ouster_show.py diff --git a/GEMstack/offboard/calibration/camera_info.py b/GEMstack/offboard/calibration/camera_info.py index 3b45110a5..5610efb7f 100644 --- a/GEMstack/offboard/calibration/camera_info.py +++ b/GEMstack/offboard/calibration/camera_info.py @@ -7,91 +7,28 @@ import pickle import image_geometry - -# OpenCV and cv2 bridge -import cv2 -from cv_bridge import CvBridge import numpy as np import os import time -lidar_points = None camera_image = None -depth_image = None -bridge = CvBridge() - -def lidar_callback(lidar : PointCloud2): - global lidar_points - lidar_points = lidar def camera_callback(info : CameraInfo): global camera_image camera_image = info -def pc2_to_numpy(pc2_msg, want_rgb = False): - gen = pc2.read_points(pc2_msg, skip_nans=True) - if want_rgb: - xyzpack = np.array(list(gen),dtype=np.float32) - if xyzpack.shape[1] != 4: - raise ValueError("PointCloud2 does not have points") - xyzrgb = np.empty((xyzpack.shape[0],6)) - xyzrgb[:,:3] = xyzpack[:,:3] - for i,x in enumerate(xyzpack): - rgb = x[3] - # cast float32 to int so that bitwise operations are possible - s = struct.pack('>f' ,rgb) - i = struct.unpack('>l',s)[0] - # you can get back the float value by the inverse operations - pack = ctypes.c_uint32(i).value - r = (pack & 0x00FF0000)>> 16 - g = (pack & 0x0000FF00)>> 8 - b = (pack & 0x000000FF) - #r,g,b values in the 0-255 range - xyzrgb[i,3:] = (r,g,b) - return xyzrgb - else: - return np.array(list(gen),dtype=np.float32)[:,:3] - -def save_scan(cam_path): +def get_intrinsics(): model = image_geometry.PinholeCameraModel() model.fromCameraInfo(camera_image) print(model.intrinsicMatrix()) - # with open(cam_path, "w") as file: - # pickle.dump(, file) - # print("Saving scan to", cam_path) def main(folder='data',start_index=1): rospy.init_node("capture_cam_info",disable_signals=True) caminfo_sub = rospy.Subscriber("/oak/rgb/camera_info", CameraInfo, camera_callback) - index = start_index - print("Press any key to:") - print(" store camera info ") - print("Press Escape or Ctrl+C to quit") while True: if camera_image: time.sleep(1) - # key = cv2.waitKey(0) - # if key == -1: - # #nothing - # pass - # elif key == 27: - # #escape - # break - # else: - # print("this is what we want") - # if lidar_points is None or camera_image is None: - # print("Missing some messages?") - # else: - # files = [ - # os.path.join(folder,'lidar{}.npz'.format(index)), - # os.path.join(folder,'color{}.png'.format(index))] - # save_scan(*files) - # index += 1 - - files = [ - os.path.join(folder,'cam_info{}.txt'.format(index))] - save_scan(*files) - index += 1 + get_intrinsics() if __name__ == '__main__': import sys diff --git a/GEMstack/offboard/calibration/capture_ouster_oak.py b/GEMstack/offboard/calibration/capture_ouster_oak.py index a246a1ff0..88cb38bc3 100644 --- a/GEMstack/offboard/calibration/capture_ouster_oak.py +++ b/GEMstack/offboard/calibration/capture_ouster_oak.py @@ -14,6 +14,7 @@ lidar_points = None camera_image = None +depth = None depth_image = None bridge = CvBridge() @@ -25,6 +26,10 @@ def camera_callback(img : Image): global camera_image camera_image = img +def depth_callback(img : Image): + global depth + depth = img + def pc2_to_numpy(pc2_msg, want_rgb = False): gen = pc2.read_points(pc2_msg, skip_nans=True) if want_rgb: @@ -49,46 +54,40 @@ def pc2_to_numpy(pc2_msg, want_rgb = False): else: return np.array(list(gen),dtype=np.float32)[:,:3] -def save_scan(lidar_fn,color_fn): +def save_scan(lidar_fn,color_fn,depth_fn): print("Saving scan to",lidar_fn,color_fn) - pc = pc2_to_numpy(lidar_points,want_rgb=False) + + pc = pc2_to_numpy(lidar_points,want_rgb=False) # convert lidar feed to numpy np.savez(lidar_fn,pc) cv2.imwrite(color_fn,bridge.imgmsg_to_cv2(camera_image)) + dimage = bridge.imgmsg_to_cv2(depth_image) + dimage_non_nan = dimage[np.isfinite(dimage)] + print("Depth range",np.min(dimage_non_nan),np.max(dimage_non_nan)) + dimage = np.nan_to_num(dimage,nan=0,posinf=0,neginf=0) + dimage = (dimage/4000*0xffff) + print("Depth pixel range",np.min(dimage),np.max(dimage)) + dimage = dimage.astype(np.uint16) + cv2.imwrite(depth_fn,dimage) + def main(folder='data',start_index=1): - rospy.init_node("capture_lidar_zed",disable_signals=True) + rospy.init_node("caoture_ouster_oak",disable_signals=True) lidar_sub = rospy.Subscriber("/ouster/points", PointCloud2, lidar_callback) camera_sub = rospy.Subscriber("/oak/rgb/image_raw", Image, camera_callback) + depth_sub = rospy.Subscriber("/oak/rgb/image_raw/compressedDepth", Image, depth_callback) index = start_index - print("Press any key to:") - print(" store lidar point clouds as npz") - print(" store color images as png") - print("Press Escape or Ctrl+C to quit") + print(" Storing lidar point clouds as npz") + print(" Storing color images as png") + print(" Storing depth images as tif") + print(" Ctrl+C to quit") while True: if camera_image: cv2.imshow("result",bridge.imgmsg_to_cv2(camera_image)) time.sleep(1) - # key = cv2.waitKey(0) - # if key == -1: - # #nothing - # pass - # elif key == 27: - # #escape - # break - # else: - # print("this is what we want") - # if lidar_points is None or camera_image is None: - # print("Missing some messages?") - # else: - # files = [ - # os.path.join(folder,'lidar{}.npz'.format(index)), - # os.path.join(folder,'color{}.png'.format(index))] - # save_scan(*files) - # index += 1 - files = [ os.path.join(folder,'lidar{}.npz'.format(index)), - os.path.join(folder,'color{}.png'.format(index))] + os.path.join(folder,'color{}.png'.format(index)), + os.path.join(folder,'depth{}.tif'.format(index))] save_scan(*files) index += 1 diff --git a/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py b/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py new file mode 100644 index 000000000..7513efe53 --- /dev/null +++ b/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py @@ -0,0 +1,173 @@ +from klampt import vis +from klampt.math import so3,se3 +from klampt.vis.colorize import colorize +from klampt import PointCloud,Geometry3D +from klampt.io import numpy_convert +from klampt.model.sensing import image_to_points +from klampt.model.create import bbox +import klampt +import cv2 +import os +import numpy as np +import math +import time + +#uncalibrated values -- TODO: load these from a calibration file +zed_K = np.array([[684.8333, 0.0, 573.371], + [0.0, 684.609, 363.7], + [0.0, 0.0, 1.0]]) +zed_intrinsics = [zed_K[0,0],zed_K[1,1],zed_K[0,2],zed_K[1,2]] +zed_w = 1152 +zed_h = 720 + + +def main(folder): + lidar_xform = se3.identity() + zed_xform = (so3.from_ndarray(np.array([[0,0,1],[-1,0,0],[0,-1,0]])),[0,0,0]) + lidar_pattern = os.path.join(folder,"lidar{}.npz") + color_pattern = os.path.join(folder,"color{}.png") + depth_pattern = os.path.join(folder,"depth{}.tif") + data = {} + def load_and_show_scan(idx): + arr_compressed = np.load(lidar_pattern.format(idx)) + arr = arr_compressed['arr_0'] + arr_compressed.close() + pc = numpy_convert.from_numpy(arr,'PointCloud') + pc = colorize(pc,'z','plasma') + data['lidar'] = Geometry3D(pc) + + try: + color = cv2.imread(color_pattern.format(idx)) + depth = cv2.imread(depth_pattern.format(idx),cv2.IMREAD_UNCHANGED) + depth = depth.astype(np.float32) + print("depth range",np.min(depth),np.max(depth)) + zed_xfov = 2*np.arctan(zed_w/(2*zed_intrinsics[0])) + zed_yfov = 2*np.arctan(zed_h/(2*zed_intrinsics[1])) + print("estimated zed horizontal FOV",math.degrees(zed_xfov),"deg") + pc = image_to_points(depth,color,zed_xfov,zed_yfov,depth_scale=4000.0/0xffff, points_format='PointCloud') + except Exception as e: + print("Error loading zed data:",e) + pc = PointCloud() + + data['zed'] = Geometry3D(pc) + data['lidar'].setCurrentTransform(*lidar_xform) + data['zed'].setCurrentTransform(*zed_xform) + vis.add('lidar',data['lidar']) + vis.add('zed',data['zed']) + + data['index'] = 1 + def increment_index(): + data['index'] += 1 + try: + load_and_show_scan(data['index']) + except Exception: + data['index'] -= 1 + return + def decrement_index(): + data['index'] -= 1 + try: + load_and_show_scan(data['index']) + except Exception: + data['index'] += 1 + return + def print_xforms(): + print("lidar:") + print("rotation:",so3.ndarray(lidar_xform[0])) + print("position:",lidar_xform[1]) + print("zed:") + print("rotation:",so3.ndarray(zed_xform[0])) + print("position:",zed_xform[1]) + + point_selection = (4,0,0) + box_selection = [(3.5,-0.5,-0.5),(4.5,0.5,0.5)] + box_geometry = bbox(box_selection[0],box_selection[1],type='GeometricPrimitive') + data['selection_widget'] = None + + def select_point(): + if data['selection_widget'] is not None: + vis.remove(data['selection_widget']) + data['selection_widget'] = 'point_widget' + vis.add('point_widget',point_selection) + vis.edit('point_widget') + + def select_box(): + if data['selection_widget'] is not None: + vis.remove(data['selection_widget']) + data['selection_widget'] = 'box_widget' + vis.add('box_widget',box_geometry,color=(1.0,0.5,0,0.5)) + vis.add('box_widget_handle1',box_selection[0]) + vis.edit('box_widget_handle1') + vis.add('box_widget_handle2',box_selection[1]) + vis.edit('box_widget_handle2') + + def print_selection(): + if data['selection_widget'] is not None: + if data['selection_widget'] == 'point_widget': + print("Target point:",point_selection) + lidar = data['lidar'] # type: Geometry3D + pts = lidar.getPointCloud().getPoints() + pts = pts @ so3.ndarray(lidar_xform[0]).T + np.array(lidar_xform[1]) + dist_2 = np.sum((pts - point_selection)**2, axis=1) + closest_dist2 = np.min(dist_2) + closest_ind = np.argmin(dist_2) + print("Closest lidar point is",pts[closest_ind],"index",closest_ind,"at distance",math.sqrt(closest_dist2)) + else: + print("Target box:",box_selection) + lidar = data['lidar'] # type: Geometry3D + pts = lidar.getPointCloud().getPoints() + pts = pts @ so3.ndarray(lidar_xform[0]).T + np.array(lidar_xform[1]) + mask = np.logical_and(np.all(pts >= box_selection[0],axis=1),np.all(pts <= box_selection[1],axis=1)) + print("Number of lidar points in box:",np.sum(mask)) + print("Indices of lidar points in box:",np.where(mask)[0]) + print("Points in box",pts[mask]) + else: + print("No selection") + + vis.addAction(select_point,'Select with point','s','Select a point by dragging a point widget') + vis.addAction(select_box,'Select with box','b','Select multiple points with a box widget') + vis.addAction(print_selection,'Print selection','q') + + vis.addAction(increment_index,"Increment index",'=') + vis.addAction(decrement_index,"Decrement index",'-') + vis.addAction(print_xforms,'Print transforms','p') + load_and_show_scan(1) + vis.add('zed_xform',zed_xform) + vis.add('lidar_xform',lidar_xform) + vis.edit('zed_xform') + vis.edit('lidar_xform') + vis.show() + while vis.shown(): + lidar_xform = vis.getItemConfig('lidar_xform') + lidar_xform = lidar_xform[:9],lidar_xform[9:] + zed_xform = vis.getItemConfig('zed_xform') + zed_xform = zed_xform[:9],zed_xform[9:] + if data['selection_widget'] == 'point_widget': + point_selection = vis.getItemConfig('point_widget') + elif data['selection_widget'] == 'box_widget': + handle1 = vis.getItemConfig('box_widget_handle1') + handle2 = vis.getItemConfig('box_widget_handle2') + lower = [min(handle1[i],handle2[i]) for i in range(3)] + upper = [max(handle1[i],handle2[i]) for i in range(3)] + box_selection = [lower,upper] + box_geometry = bbox(lower,upper,type='GeometricPrimitive') + vis.add('box_widget',box_geometry,color=(1.0,0.5,0,0.5)) + data['lidar'].setCurrentTransform(*lidar_xform) + data['zed'].setCurrentTransform(*zed_xform) + time.sleep(0.02) + vis.kill() + +if __name__ == '__main__': + import sys + folder = 'data' + if len(sys.argv) >= 2: + folder = sys.argv[1] + if len(sys.argv) >= 3: + calib = sys.argv[2] + import yaml + with open(calib,'r') as f: + config = yaml.load(f,yaml.SafeLoader) + zed_K = np.array(config['K']).reshape((3,3)) + zed_intrinsics = [zed_K[0,0],zed_K[1,1],zed_K[0,2],zed_K[1,2]] + zed_w = config['width'] + zed_height = config['height'] + main(folder) From 2a0f9c2b82ea3906669d7eddff7e123a8907a7cc Mon Sep 17 00:00:00 2001 From: aadarshhegde Date: Sat, 8 Feb 2025 17:24:37 -0600 Subject: [PATCH 31/71] working depth image capture --- .../offboard/calibration/capture_ouster_oak.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/GEMstack/offboard/calibration/capture_ouster_oak.py b/GEMstack/offboard/calibration/capture_ouster_oak.py index 88cb38bc3..3c9e1e6af 100644 --- a/GEMstack/offboard/calibration/capture_ouster_oak.py +++ b/GEMstack/offboard/calibration/capture_ouster_oak.py @@ -27,8 +27,8 @@ def camera_callback(img : Image): camera_image = img def depth_callback(img : Image): - global depth - depth = img + global depth_image + depth_image = img def pc2_to_numpy(pc2_msg, want_rgb = False): gen = pc2.read_points(pc2_msg, skip_nans=True) @@ -71,19 +71,19 @@ def save_scan(lidar_fn,color_fn,depth_fn): cv2.imwrite(depth_fn,dimage) def main(folder='data',start_index=1): - rospy.init_node("caoture_ouster_oak",disable_signals=True) + rospy.init_node("capture_ouster_oak",disable_signals=True) lidar_sub = rospy.Subscriber("/ouster/points", PointCloud2, lidar_callback) camera_sub = rospy.Subscriber("/oak/rgb/image_raw", Image, camera_callback) - depth_sub = rospy.Subscriber("/oak/rgb/image_raw/compressedDepth", Image, depth_callback) - index = start_index + depth_sub = rospy.Subscriber("/oak/stereo/image_raw", Image, depth_callback) + index = 0 print(" Storing lidar point clouds as npz") print(" Storing color images as png") print(" Storing depth images as tif") print(" Ctrl+C to quit") while True: - if camera_image: + if camera_image and depth_image: cv2.imshow("result",bridge.imgmsg_to_cv2(camera_image)) - time.sleep(1) + time.sleep(.5) files = [ os.path.join(folder,'lidar{}.npz'.format(index)), os.path.join(folder,'color{}.png'.format(index)), From d0771faa329ee205daec5eb4a0e90f118713a1dd Mon Sep 17 00:00:00 2001 From: AadarshHegde123 Date: Mon, 10 Feb 2025 09:13:55 -0800 Subject: [PATCH 32/71] visualization for lidar + oak working, but no intrinsics considered --- .../calibration/klampt_lidar_ouster_show.py | 22 ++++++++++++++----- data/.gitignore | 0 data/README.md | 0 3 files changed, 16 insertions(+), 6 deletions(-) mode change 100644 => 100755 data/.gitignore mode change 100644 => 100755 data/README.md diff --git a/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py b/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py index 7513efe53..11a9fcbcc 100644 --- a/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py +++ b/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py @@ -36,7 +36,7 @@ def load_and_show_scan(idx): pc = colorize(pc,'z','plasma') data['lidar'] = Geometry3D(pc) - try: + try: # might need some modifications to work with our code color = cv2.imread(color_pattern.format(idx)) depth = cv2.imread(depth_pattern.format(idx),cv2.IMREAD_UNCHANGED) depth = depth.astype(np.float32) @@ -44,16 +44,26 @@ def load_and_show_scan(idx): zed_xfov = 2*np.arctan(zed_w/(2*zed_intrinsics[0])) zed_yfov = 2*np.arctan(zed_h/(2*zed_intrinsics[1])) print("estimated zed horizontal FOV",math.degrees(zed_xfov),"deg") - pc = image_to_points(depth,color,zed_xfov,zed_yfov,depth_scale=4000.0/0xffff, points_format='PointCloud') + print(f"Depth image shape: {depth.shape}, dtype: {depth.dtype}, min: {np.min(depth)}, max: {np.max(depth)}") + print(f"Color image shape: {color.shape}, dtype: {color.dtype}, min: {np.min(color)}, max: {np.max(color)}") + image_to_points( + depth, + color, + intrinsics=None, + xfov=zed_xfov, + yfov=zed_yfov, + depth_scale=4000.0 / 0xffff + ) + except Exception as e: print("Error loading zed data:",e) pc = PointCloud() - data['zed'] = Geometry3D(pc) + data['oak'] = Geometry3D(pc) data['lidar'].setCurrentTransform(*lidar_xform) - data['zed'].setCurrentTransform(*zed_xform) + data['oak'].setCurrentTransform(*zed_xform) vis.add('lidar',data['lidar']) - vis.add('zed',data['zed']) + vis.add('oak',data['oak']) data['index'] = 1 def increment_index(): @@ -152,7 +162,7 @@ def print_selection(): box_geometry = bbox(lower,upper,type='GeometricPrimitive') vis.add('box_widget',box_geometry,color=(1.0,0.5,0,0.5)) data['lidar'].setCurrentTransform(*lidar_xform) - data['zed'].setCurrentTransform(*zed_xform) + data['oak'].setCurrentTransform(*zed_xform) time.sleep(0.02) vis.kill() diff --git a/data/.gitignore b/data/.gitignore old mode 100644 new mode 100755 diff --git a/data/README.md b/data/README.md old mode 100644 new mode 100755 From 3c6bdc1b7ef957d059aa485d50020f330df39985 Mon Sep 17 00:00:00 2001 From: Averyyy <58547284+Averyyy@users.noreply.github.com> Date: Mon, 10 Feb 2025 15:14:00 -0600 Subject: [PATCH 33/71] feat: init team folder --- GEMstack/scripts/__init__.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 GEMstack/scripts/__init__.py diff --git a/GEMstack/scripts/__init__.py b/GEMstack/scripts/__init__.py new file mode 100644 index 000000000..e69de29bb From e23a5cb6fcdd15eaf47340c8b9afcaa88146cef7 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Tue, 11 Feb 2025 05:34:21 +0000 Subject: [PATCH 34/71] script to fit objects in a line --- .../calibration/make_gem_e4_ouster.py | 67 +++++++++++++++++++ 1 file changed, 67 insertions(+) create mode 100644 GEMstack/offboard/calibration/make_gem_e4_ouster.py diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster.py b/GEMstack/offboard/calibration/make_gem_e4_ouster.py new file mode 100644 index 000000000..08f4ac1af --- /dev/null +++ b/GEMstack/offboard/calibration/make_gem_e4_ouster.py @@ -0,0 +1,67 @@ +#%% +import os +import sys +os.getcwd() + +#%% +pbg = '/mount/wp/GEMstack/data/lidar76.npz' +pbgAndLine = '/mount/wp/GEMstack/data/lidar78.npz' + +#%% load and wash data +import numpy as np +#load null scene -- no added objects +bg = np.load(pbg)['arr_0'] +#load data pc -- with added object +bgAndLine = np.load(pbgAndLine)['arr_0'] +#remove (0,0,0)'s +bg = bg[~np.all(bg == 0, axis=1)] +bgAndLine = bgAndLine[~np.all(bgAndLine == 0, axis=1)] + +#%% crop to only keep a frontal box area +def crop(pc,ix=None,iy=None,iz=None): + mask = True + for dim,intervel in zip([0,1,2],[ix,iy,iz]): + if not intervel: continue + d,u = intervel + mask &= pc[:,dim] >= d + mask &= pc[:,dim] <= u + print(f'points left after cropping {dim}\'th dim',mask.sum()) + return pc[mask] +# +area = (-0,5),(-1,1) +bg = crop(bg,*area) +bgAndLine = crop(bgAndLine,*area) +print(bg.shape) +print(bgAndLine.shape) + +#%% visualize cropped null scene and data scene +import pyvista as pv +import panel as pn +import matplotlib.pyplot as plt +def vispc(pc): + pv.set_jupyter_backend('client') + cloud = pv.PolyData(pc) + plotter = pv.Plotter(notebook=True) + plotter.add_mesh(cloud, render_points_as_spheres=True, point_size=2, color='blue') + plotter.camera.position = (-20,0,20) + plotter.camera.focal_point = (0,0,0) + plotter.show() + +vispc(bg) +vispc(bgAndLine) +#%% take difference to only keep added object +from scipy.spatial import cKDTree +tree = cKDTree(bg) + +tolerance = 0.08 +# Find points in pc1 that do not have a match in pc2 within the tolerance +idiff = [] +for i, point in enumerate(bgAndLine): + _, idx = tree.query(point) # Find the nearest neighbor in pc2 + distance = np.linalg.norm(point - bg[idx]) # Compute the distance + if distance > tolerance: # If the nearest neighbor is outside the tolerance + idiff.append(i) + +diff = np.array(bgAndLine[diff]) +#visualize +vispc(diff) From 555cb550b31866fcc42edbae5fbb1a40d2b2950c Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Tue, 11 Feb 2025 08:14:40 +0000 Subject: [PATCH 35/71] calibration Lidar to Vehicle. Running OK. To be improved --- .../calibration/make_gem_e4_ouster.py | 164 +++++++++++++----- 1 file changed, 123 insertions(+), 41 deletions(-) diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster.py b/GEMstack/offboard/calibration/make_gem_e4_ouster.py index 08f4ac1af..aaaf2240d 100644 --- a/GEMstack/offboard/calibration/make_gem_e4_ouster.py +++ b/GEMstack/offboard/calibration/make_gem_e4_ouster.py @@ -2,37 +2,21 @@ import os import sys os.getcwd() +VIS = False +#%% things to extract +tx,ty,tz,rx,ry,rz = [None] * 6 #%% -pbg = '/mount/wp/GEMstack/data/lidar76.npz' +pbg = '/mount/wp/GEMstack/data/lidar76.npz' #null scene pbgAndLine = '/mount/wp/GEMstack/data/lidar78.npz' #%% load and wash data import numpy as np -#load null scene -- no added objects -bg = np.load(pbg)['arr_0'] -#load data pc -- with added object -bgAndLine = np.load(pbgAndLine)['arr_0'] -#remove (0,0,0)'s -bg = bg[~np.all(bg == 0, axis=1)] -bgAndLine = bgAndLine[~np.all(bgAndLine == 0, axis=1)] -#%% crop to only keep a frontal box area -def crop(pc,ix=None,iy=None,iz=None): - mask = True - for dim,intervel in zip([0,1,2],[ix,iy,iz]): - if not intervel: continue - d,u = intervel - mask &= pc[:,dim] >= d - mask &= pc[:,dim] <= u - print(f'points left after cropping {dim}\'th dim',mask.sum()) - return pc[mask] -# -area = (-0,5),(-1,1) -bg = crop(bg,*area) -bgAndLine = crop(bgAndLine,*area) -print(bg.shape) -print(bgAndLine.shape) +bg = np.load(pbg)['arr_0'] # load null scene -- no added objects +bgAndLine = np.load(pbgAndLine)['arr_0'] # load data pc -- with added object +bg = bg[~np.all(bg == 0, axis=1)] # remove (0,0,0)'s +bgAndLine = bgAndLine[~np.all(bgAndLine == 0, axis=1)] #%% visualize cropped null scene and data scene import pyvista as pv @@ -45,23 +29,121 @@ def vispc(pc): plotter.add_mesh(cloud, render_points_as_spheres=True, point_size=2, color='blue') plotter.camera.position = (-20,0,20) plotter.camera.focal_point = (0,0,0) + plotter.show_axes() plotter.show() +if VIS: + vispc(bg) + vispc(bgAndLine) + + +#%%============================================================== +#======================= util functions ========================= +#================================================================ +def crop(pc,ix=None,iy=None,iz=None): + # crop a subrectangle in a pointcloud + # usage: crop(pc, ix = (x_min,x_max), ...) + # return: array(N,3) + mask = True + for dim,intervel in zip([0,1,2],[ix,iy,iz]): + if not intervel: continue + d,u = intervel + mask &= pc[:,dim] >= d + mask &= pc[:,dim] <= u + print(f'points left after cropping {dim}\'th dim',mask.sum()) + return pc[mask] + +from sklearn.linear_model import RANSACRegressor +from sklearn.linear_model import LinearRegression +def fit_plane_ransac(pc,tol=0.01): + # fit a plane in a pointcloud + # and visualize inliers + # pc: np array (N,3). the pointcloud + # tol: the tolerance. default 0.01 + # return: float, float, float, array(N,3) + # ^: coeff1, coeff2, intercept toward the plane, inliers of the fit + model = RANSACRegressor(LinearRegression(), residual_threshold=tol) + model.fit(pc[:,:2], pc[:,2]) + a, b = model.estimator_.coef_ + inter = model.estimator_.intercept_ + return a,b,inter,cropped_bg[model.inlier_mask_] -vispc(bg) -vispc(bgAndLine) -#%% take difference to only keep added object from scipy.spatial import cKDTree -tree = cKDTree(bg) - -tolerance = 0.08 -# Find points in pc1 that do not have a match in pc2 within the tolerance -idiff = [] -for i, point in enumerate(bgAndLine): - _, idx = tree.query(point) # Find the nearest neighbor in pc2 - distance = np.linalg.norm(point - bg[idx]) # Compute the distance - if distance > tolerance: # If the nearest neighbor is outside the tolerance - idiff.append(i) - -diff = np.array(bgAndLine[diff]) -#visualize -vispc(diff) +def pc_diff(pc0,pc1,tol=0.1): + # remove points in pc0 from pc1 s.t. euclidian distance is within tolerance + # return: array(N,3) + tree = cKDTree(pc0) + diff = [] + for i, point in enumerate(pc1): + _, idx = tree.query(point) + distance = np.linalg.norm(point - pc0[idx]) # Compute the distance + if distance > tol: + diff.append(point) + # tree = cKDTree(pc1) + # for i, point in enumerate(pc0): + # _, idx = tree.query(point) + # distance = np.linalg.norm(point - pc1[idx]) # Compute the distance + # if distance > tol: + # diff.append(point) + return np.array(diff) + + + +#%%============================================================== +#========================== tx ty rz ============================ +#================================================================ +#%% crop to only keep a frontal box area +area = (-0,10),(-2,2),(-3,1) +cropped_bg = crop(bg,*area) +cropped_bgAndLine = crop(bgAndLine,*area) +print(cropped_bg.shape) +print(cropped_bgAndLine.shape) + +#%% Take difference to only keep added object +diff = pc_diff(cropped_bg,cropped_bgAndLine) +if VIS: + vispc(diff) #visualize diff, hopefully the added objects + +# %% use the added objects to find rz. +# TODO after dataset retake +# right now we assume tx = ty = 0 and \ +# just use median to find a headding direction +tx = ty = 0 +hx,hy = np.median(diff,axis=0)[:2] +rz = -np.arctan2(hy,hx) + + + + +#%%============================================================== +#========================= tz rx ry ============================= +#================================================================ +# %% this time we crop to keep the ground +cropped_bg = crop(bg,iz = (-3,-2)) +if VIS: + print(cropped_bg.shape) + vispc(cropped_bg) + +#%% +from math import sqrt +a, b, tz, inliers = fit_plane_ransac(cropped_bg,tol=0.001) +if VIS: + vispc(inliers) +normv = np.array([a, b, -1]) +normv /= np.linalg.norm(normv) +nx,ny,nz = normv +ry = np.arctan2(-nx, sqrt(ny**2 + nz**2)) +rx = np.arctan2(ny,sqrt(nx**2 + nz**2)) + + + + + +#%% visualize calibrated pointcloud +if VIS: + from scipy.spatial.transform import Rotation as R + rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() + + calibrated_bgAndLine = bgAndLine @ rot.T + [tx,ty,tz] + + vispc(calibrated_bgAndLine) +# %% From 734f0b3a8639d49654ccdd36383fe5130df8c22c Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Tue, 11 Feb 2025 08:27:00 +0000 Subject: [PATCH 36/71] update translation_z using magic numbers we measured --- GEMstack/offboard/calibration/make_gem_e4_ouster.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster.py b/GEMstack/offboard/calibration/make_gem_e4_ouster.py index aaaf2240d..ba879d171 100644 --- a/GEMstack/offboard/calibration/make_gem_e4_ouster.py +++ b/GEMstack/offboard/calibration/make_gem_e4_ouster.py @@ -7,6 +7,7 @@ #%% things to extract tx,ty,tz,rx,ry,rz = [None] * 6 #%% +#TODO make into command line arguments pbg = '/mount/wp/GEMstack/data/lidar76.npz' #null scene pbgAndLine = '/mount/wp/GEMstack/data/lidar78.npz' @@ -125,7 +126,11 @@ def pc_diff(pc0,pc1,tol=0.1): #%% from math import sqrt -a, b, tz, inliers = fit_plane_ransac(cropped_bg,tol=0.001) +a, b, height, inliers = fit_plane_ransac(cropped_bg,tol=0.001) +# TODO MAGIC NUMBER WARNING +real_tz = 203 #https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ +real_height = 203 + 27.94 # 11 inches that we measured +tz = height * real_tz/real_height if VIS: vispc(inliers) normv = np.array([a, b, -1]) @@ -147,3 +152,7 @@ def pc_diff(pc0,pc1,tol=0.1): vispc(calibrated_bgAndLine) # %% +print(f""" +translation: ({tx,ty,tz}) +rotation: ({rx,ry,rz}) +""") From 6a38ed0a0a18c03bbff3d1e2077261b1b0b80a50 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Tue, 11 Feb 2025 20:10:26 +0000 Subject: [PATCH 37/71] enhenced visualization --- .../calibration/make_gem_e4_ouster.py | 179 ++++++++++++------ 1 file changed, 123 insertions(+), 56 deletions(-) diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster.py b/GEMstack/offboard/calibration/make_gem_e4_ouster.py index ba879d171..68ae643fa 100644 --- a/GEMstack/offboard/calibration/make_gem_e4_ouster.py +++ b/GEMstack/offboard/calibration/make_gem_e4_ouster.py @@ -1,6 +1,7 @@ #%% import os import sys +import math os.getcwd() VIS = False @@ -8,7 +9,7 @@ tx,ty,tz,rx,ry,rz = [None] * 6 #%% #TODO make into command line arguments -pbg = '/mount/wp/GEMstack/data/lidar76.npz' #null scene +pbg = '/mount/wp/GEMstack/data/lidar70.npz' #null scene pbgAndLine = '/mount/wp/GEMstack/data/lidar78.npz' #%% load and wash data @@ -23,19 +24,47 @@ import pyvista as pv import panel as pn import matplotlib.pyplot as plt -def vispc(pc): +def vis(ratio = 1): pv.set_jupyter_backend('client') - cloud = pv.PolyData(pc) plotter = pv.Plotter(notebook=True) - plotter.add_mesh(cloud, render_points_as_spheres=True, point_size=2, color='blue') - plotter.camera.position = (-20,0,20) + plotter.camera.position = (-20*ratio,0,20*ratio) plotter.camera.focal_point = (0,0,0) plotter.show_axes() - plotter.show() -if VIS: - vispc(bg) - vispc(bgAndLine) + class foo: + def add_pc(self,pc,ratio=ratio,**kargs): + print(ratio) + plotter.add_mesh( + pv.PolyData(pc*ratio), + render_points_as_spheres=True, + point_size=2, + **kargs) + return self + def add_line(self,p1,p2,ratio=ratio): + plotter.add_mesh( + pv.Line(p1*ratio,p2*ratio), + color='red', + line_width=1) + return self + def add_box(self,bound,trans,ratio=ratio): + print(ratio) + l,w,h = map(lambda x:x*ratio,bound) + box = pv.Box(bounds=(-l/2,l/2,-w/2,w/2,-h/2,h/2)) + print(box) + print(*map(lambda x:x*ratio,trans)) + box = box.translate(list(map(lambda x:x*ratio,trans))) + print(box) + plotter.add_mesh(box, color='yellow',show_edges=True) + return self + def show(self): + plotter.show() + return self + + return foo() + +if VIS: + vis().add_pc(bg,color='blue').show() + vis().add_pc(bgAndLine,color='blue').show() #%%============================================================== #======================= util functions ========================= @@ -56,17 +85,30 @@ def crop(pc,ix=None,iy=None,iz=None): from sklearn.linear_model import RANSACRegressor from sklearn.linear_model import LinearRegression def fit_plane_ransac(pc,tol=0.01): - # fit a plane in a pointcloud - # and visualize inliers - # pc: np array (N,3). the pointcloud + # fit a line/plane/hyperplane in a pointcloud + # pc: np array (N,D). the pointcloud # tol: the tolerance. default 0.01 - # return: float, float, float, array(N,3) - # ^: coeff1, coeff2, intercept toward the plane, inliers of the fit model = RANSACRegressor(LinearRegression(), residual_threshold=tol) - model.fit(pc[:,:2], pc[:,2]) - a, b = model.estimator_.coef_ + model.fit(pc[:,:-1], pc[:,-1]) + a = model.estimator_.coef_ inter = model.estimator_.intercept_ - return a,b,inter,cropped_bg[model.inlier_mask_] + class foo: + def plot(self): + inliers = pc[model.inlier_mask_] + if pc.shape[1] == 2: + plt.scatter(pc[:,0], pc[:,1], color='blue', marker='o', s=1) + plt.scatter(inliers[:,0], inliers[:,1], color='red', marker='o', s=1) + x_line = np.linspace(0, 7, 100).reshape(-1,1) + plt.plot(x_line, x_line * a[0] + inter, color='red', label='Fitted Line') + elif pc.shape[1] == 3: + vis().add_pc(pc).add_pc(inliers,color='red').show() + return self + + def results(self): + # return: array(D-1), float, array(N,3) + # ^: , coeffs, intercept toward the plane, inliers of the fit + return a,inter + return foo() from scipy.spatial import cKDTree def pc_diff(pc0,pc1,tol=0.1): @@ -79,21 +121,53 @@ def pc_diff(pc0,pc1,tol=0.1): distance = np.linalg.norm(point - pc0[idx]) # Compute the distance if distance > tol: diff.append(point) - # tree = cKDTree(pc1) - # for i, point in enumerate(pc0): - # _, idx = tree.query(point) - # distance = np.linalg.norm(point - pc1[idx]) # Compute the distance - # if distance > tol: - # diff.append(point) + tree = cKDTree(pc1) + for i, point in enumerate(pc0): + _, idx = tree.query(point) + distance = np.linalg.norm(point - pc1[idx]) # Compute the distance + if distance > tol: + diff.append(point) return np.array(diff) + + + +#%%============================================================== +#========================= tz rx ry ============================= +#================================================================ +# %% this time we crop to keep the ground +cropped_bg = crop(bg,iz = (-3,-2)) +if VIS: + print(cropped_bg.shape) + vis().add_pc(cropped_bg,color='blue').show() + +#%% +from math import sqrt +fit = fit_plane_ransac(cropped_bg,tol=0.01) +c, inter = fit.results() +normv = np.array([c[0], c[1], -1]) +normv /= np.linalg.norm(normv) +nx,ny,nz = normv +height = nz * inter +# TODO MAGIC NUMBER WARNING +real_tz = 203 #https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ +real_height = 203 + 27.94 # 11 inches that we measured +ratio = height / real_height +tz = ratio * real_tz +if VIS: + fit.plot() +ry = math.atan2(nx, sqrt(ny**2 + nz**2)) +rx = math.atan2(-ny,sqrt(nx**2 + nz**2)) + + + #%%============================================================== #========================== tx ty rz ============================ #================================================================ #%% crop to only keep a frontal box area -area = (-0,10),(-2,2),(-3,1) +area = (-0,7),(-1,1),(-3,1) cropped_bg = crop(bg,*area) cropped_bgAndLine = crop(bgAndLine,*area) print(cropped_bg.shape) @@ -102,45 +176,33 @@ def pc_diff(pc0,pc1,tol=0.1): #%% Take difference to only keep added object diff = pc_diff(cropped_bg,cropped_bgAndLine) if VIS: - vispc(diff) #visualize diff, hopefully the added objects + vis().add_pc(diff,color='blue').show() #visualize diff, hopefully the added objects # %% use the added objects to find rz. # TODO after dataset retake # right now we assume tx = ty = 0 and \ # just use median to find a headding direction -tx = ty = 0 -hx,hy = np.median(diff,axis=0)[:2] -rz = -np.arctan2(hy,hx) - - - - -#%%============================================================== -#========================= tz rx ry ============================= -#================================================================ -# %% this time we crop to keep the ground -cropped_bg = crop(bg,iz = (-3,-2)) +fit = fit_plane_ransac(diff[:,:2],tol=0.6) +c,inter = fit.results() if VIS: - print(cropped_bg.shape) - vispc(cropped_bg) + fit.plot() +# tx = ty = 0 +# hx,hy = np.median(diff,axis=0)[:2] +# rz = -np.arctan2(hy,hx) +real_tx = 256 - 146 # https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ +tx = - ratio * real_tx +ty = - inter +rz = - math.atan(c) -#%% -from math import sqrt -a, b, height, inliers = fit_plane_ransac(cropped_bg,tol=0.001) -# TODO MAGIC NUMBER WARNING -real_tz = 203 #https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ -real_height = 203 + 27.94 # 11 inches that we measured -tz = height * real_tz/real_height if VIS: - vispc(inliers) -normv = np.array([a, b, -1]) -normv /= np.linalg.norm(normv) -nx,ny,nz = normv -ry = np.arctan2(-nx, sqrt(ny**2 + nz**2)) -rx = np.arctan2(ny,sqrt(nx**2 + nz**2)) - - + from scipy.spatial.transform import Rotation as R + rot = R.from_euler('xyz',[0,0,rz]).as_matrix() + calibrated_bgAndLine = bgAndLine @ rot.T + [tx,ty,0] + line = np.arange(0,100)/100*7 + line = np.stack((line,c[0]*line+inter,np.zeros(100)),axis=1) + line = line @ rot.T + [tx,ty,0] + vis().add_pc(calibrated_bgAndLine,color='blue').add_pc(line,color='red').show() #%% visualize calibrated pointcloud @@ -149,10 +211,15 @@ def pc_diff(pc0,pc1,tol=0.1): rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() calibrated_bgAndLine = bgAndLine @ rot.T + [tx,ty,tz] - - vispc(calibrated_bgAndLine) + # projection + # calibrated_bgAndLine[:,2] = 0 + v = vis(ratio=1/ratio) + v.add_pc(calibrated_bgAndLine,color='blue') + v.add_box((256,61*2,203),[256/2,0,203/2],ratio=1) + v.show() # %% print(f""" translation: ({tx,ty,tz}) rotation: ({rx,ry,rz}) """) + From 88ec6bacf444ace4958187ab0d49222edc8a231d Mon Sep 17 00:00:00 2001 From: jeffbyju <59282955+jeffbyju@users.noreply.github.com> Date: Tue, 11 Feb 2025 14:28:12 -0600 Subject: [PATCH 38/71] semi correct code for icp --- .DS_Store | Bin 0 -> 8196 bytes GEMstack/.DS_Store | Bin 0 -> 6148 bytes GEMstack/offboard/calibration/icp.py | 179 +++++++++++++++++++++++++++ 3 files changed, 179 insertions(+) create mode 100644 .DS_Store create mode 100644 GEMstack/.DS_Store create mode 100644 GEMstack/offboard/calibration/icp.py diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..043038f0b65f2f5ca0e0f558b8366b72ba550cd9 GIT binary patch literal 8196 zcmeHMziSjh6n=AVyaVBqN@62ZrV?XovxW}<{MOk*NuWdT1)J5sV0db(}fT`URx-8sD zA7hne=r7~(m^tFg3QJEMJ?hrADeag6icJ`Q;8 zT)yi%Jm76v-VWWRZMsi$vuE%>3iv0_K7+rK!*y(&zFXc0HF>2EXXORD<_|zgmuUh$ z3p#|RaiEE=y^3+JXZNJ=C0K)d)07kFAgd#Rjtswy=(@CCLqmaa{;$sqeukGm)Zrno z#?sjT96KLk|HGJ@zQj1^zy7A+57yxOZ_26Bu>u`K+#3s;g;$~b6Dkj)f%D|RCAXP%Q~z%_ zfB!#ETb8|u1L8pBfJ%Bhy)6v-+B$+=m9H_n>k*Te<3O}i(Ajbv(U#+g#UF+^*GM&q YsgJpf7(x5z9|A`5{56rv2tgd)+PyJLKV8J?3dclzL%E|*98DkANw_c3II4*2umwiWd!w8Nku`oY$6Id zM+jZ;A%Jceq}bfR4zL4@<^VmrW$42IVz>t5`K>k2;%F%41`j9MtSD|YI$_+wUgxyB zb~AW-Iml!Bzhn-3M&|w<2`EDZZMcFSjIq8zm7_2)Sa3hiW|{hSJ~rn1Qu9UeT;I=3 zOCP+GJ_o%!$4>{b4VU2C>h`Bu98=qCwfdzkHJ7REC!VdVMY@(z&mc!tVf!)7Ff|?C z&NO|S$5fQhvh);>NzW3D@cPk}#Pz|reqw#q?lpDnPG&lc@ur4shLvADtNc0l&vYX5 z&w9=LTW>~wgE7BfvKe;%J<7jmfonE}L+*311MC1hU^+nW2Nw&Wr!i3|UmaMz{cO@N! zr;uxQfE}Bcl1OLhaQED_Bbvz}#wx&*w)>;=!3kwDD5`}UI lR(d&>2ecH^{|Yj&&!q!IPh+AGTQK;CfV6=tcHmDP_ylOM)Fc1^ literal 0 HcmV?d00001 diff --git a/GEMstack/offboard/calibration/icp.py b/GEMstack/offboard/calibration/icp.py new file mode 100644 index 000000000..376c285a2 --- /dev/null +++ b/GEMstack/offboard/calibration/icp.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python3 + +import os +import glob +import numpy as np +import cv2 +import open3d as o3d + +# --- Adjust these to match your intrinsics and data details --- +INTRINSICS = np.array([ + [684.83331299, 0. , 573.37109375], + [ 0. , 684.60968018, 363.70092773], + [ 0. , 0. , 1. ] +], dtype=np.float32) + +# Depth scale: how to go from raw .tif values to actual "Z" in meters. +# In your capture code, you used: dimage = (dimage/4000*0xffff) +# Possibly each pixel is stored with range 0..65535. Adjust as needed. +DEPTH_SCALE = 4000.0 # Example factor if your depth was in mm or a certain scale. + +def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): + """ + Convert a single-channel depth image into an Nx3 array of 3D points + in the camera coordinate system. + - depth_img: 2D array of type uint16 or float with depth values + - intrinsics: 3x3 camera matrix + """ + fx = intrinsics[0,0] + fy = intrinsics[1,1] + cx = intrinsics[0,2] + cy = intrinsics[1,2] + + # Indices of each pixel + h, w = depth_img.shape + i_range = np.arange(h) + j_range = np.arange(w) + jj, ii = np.meshgrid(j_range, i_range) # shape (h,w) + + # Flatten + ii = ii.flatten().astype(np.float32) + jj = jj.flatten().astype(np.float32) + depth = depth_img.flatten().astype(np.float32) + + # Filter out zero / invalid depths + valid_mask = (depth > 0) + ii = ii[valid_mask] + jj = jj[valid_mask] + depth = depth[valid_mask] + + # Reproject to 3D (camera frame) + # X = (x - cx) * Z / fx, Y = (y - cy) * Z / fy, Z = depth + # Note: Z must be in meters or consistent units + z = depth / DEPTH_SCALE # Convert from your .tif scale to real meters + x = (jj - cx) * z / fx + y = (ii - cy) * z / fy + points3d = np.stack((x, y, z), axis=-1) # shape (N,3) + + return points3d + +def load_lidar_points(npz_file: str): + """ + Load the Nx3 LiDAR points from a .npz file created by 'np.savez'. + """ + data = np.load(npz_file) + # The capture code used: np.savez(lidar_fn, pc) + # So 'data' might have the default key 'arr_0' or 'pc' if named + # Inspect data.files to see. Let's assume 'arr_0' or single key: + arr_key = data.files[0] + points = data[arr_key] + # shape check, must be Nx3 or Nx4, ... + return points + +def make_open3d_pcd(points: np.ndarray): + """ + Convert Nx3 numpy array into an Open3D point cloud object. + """ + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + return pcd + +def perform_icp(camera_pcd: o3d.geometry.PointCloud, + lidar_pcd: o3d.geometry.PointCloud): + """ + Perform local ICP alignment of camera_pcd (source) to lidar_pcd (target). + Returns a transformation 4x4 that maps camera -> lidar (or vice versa). + """ + # 1) Estimate normals if you want point-to-plane + lidar_pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=1.0, max_nn=30)) + camera_pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=1.0, max_nn=30)) + + # 2) Initial guess: identity or something better if you have one + init_guess = np.eye(4) + + # 3) Choose a threshold for inlier distance + threshold = 0.5 # adjust based on your environment scale + + # 4) Run ICP (point-to-plane or point-to-point) + print("[ICP] Running ICP alignment ...") + result = o3d.pipelines.registration.registration_icp( + camera_pcd, # source + lidar_pcd, # target + threshold, + init_guess, + estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane() + ) + + print("[ICP] Done. Fitness = %.4f, RMSE = %.4f" % (result.fitness, result.inlier_rmse)) + print("[ICP] Transformation:\n", result.transformation) + return result.transformation + +def main(folder='data'): + color_files = sorted(glob.glob(os.path.join(folder, "color*.png"))) + icp_results = [] # to store (index, transform, fitness, rmse) + + for color_path in color_files: + # Extract index from filename, e.g. color10.png -> 10 + basename = os.path.basename(color_path) + idx_str = basename.replace("color","").replace(".png","") + + if int(idx_str) in range(77): + depth_path = os.path.join(folder, f"depth{idx_str}.tif") + lidar_path = os.path.join(folder, f"lidar{idx_str}.npz") + + if not (os.path.exists(depth_path) and os.path.exists(lidar_path)): + continue + + # Load depth / convert to 3D + depth_img = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED) + if depth_img is None: + continue + camera_points = depth_to_points(depth_img, INTRINSICS) + camera_pcd = make_open3d_pcd(camera_points) + + # Load LiDAR + lidar_points = load_lidar_points(lidar_path) + lidar_pcd = make_open3d_pcd(lidar_points) + + lidar_pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=1.0, # Adjust based on your scene scale + max_nn=30 + ) + ) + + # Also estimate normals on the Camera (source) cloud (recommended) + camera_pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=1.0, + max_nn=30 + ) + ) + + # Now you can run Point-to-Plane ICP + init_guess = np.eye(4) + threshold = 0.5 # or some appropriate distance + result_icp = o3d.pipelines.registration.registration_icp( + camera_pcd, + lidar_pcd, + threshold, + init_guess, + o3d.pipelines.registration.TransformationEstimationPointToPlane() + ) + + print("ICP result:", result_icp.transformation) + print("Fitness:", result_icp.fitness, " RMSE:", result_icp.inlier_rmse) + + # After processing all frames, we can analyze or average + if len(icp_results) > 0: + # Example: pick the best frame by highest fitness + best_frame = max(icp_results, key=lambda x: x[2]) # x[2] is fitness + print("\nBest frame by fitness was index=", best_frame[0], + " with fitness=", best_frame[2], " rmse=", best_frame[3]) + else: + print("No frames processed properly.") + +if __name__ == "__main__": + main() \ No newline at end of file From a9fa4c870c528902b67e4135d13ddd364254f594 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Tue, 11 Feb 2025 21:35:27 +0000 Subject: [PATCH 39/71] better line fitting --- .../calibration/make_gem_e4_ouster.py | 21 +- .../calibration/make_gem_e4_ouster_v2.py | 227 ++++++++++++++++++ 2 files changed, 240 insertions(+), 8 deletions(-) create mode 100644 GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster.py b/GEMstack/offboard/calibration/make_gem_e4_ouster.py index 68ae643fa..3d5053740 100644 --- a/GEMstack/offboard/calibration/make_gem_e4_ouster.py +++ b/GEMstack/offboard/calibration/make_gem_e4_ouster.py @@ -152,10 +152,8 @@ def pc_diff(pc0,pc1,tol=0.1): nx,ny,nz = normv height = nz * inter # TODO MAGIC NUMBER WARNING -real_tz = 203 #https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ -real_height = 203 + 27.94 # 11 inches that we measured -ratio = height / real_height -tz = ratio * real_tz +height_axel = 0.2794 # 11 inches that we measured +tz = height - height_axel if VIS: fit.plot() ry = math.atan2(nx, sqrt(ny**2 + nz**2)) @@ -189,8 +187,7 @@ def pc_diff(pc0,pc1,tol=0.1): # tx = ty = 0 # hx,hy = np.median(diff,axis=0)[:2] # rz = -np.arctan2(hy,hx) -real_tx = 256 - 146 # https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ -tx = - ratio * real_tx +tx = - (2.56 - 1.46) # https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ ty = - inter rz = - math.atan(c) @@ -213,9 +210,9 @@ def pc_diff(pc0,pc1,tol=0.1): calibrated_bgAndLine = bgAndLine @ rot.T + [tx,ty,tz] # projection # calibrated_bgAndLine[:,2] = 0 - v = vis(ratio=1/ratio) + v = vis(ratio=100) v.add_pc(calibrated_bgAndLine,color='blue') - v.add_box((256,61*2,203),[256/2,0,203/2],ratio=1) + v.add_box((2.56,.61*2,2.03+height_axel),[2.56/2,0,(2.03+height_axel)/2]) v.show() # %% print(f""" @@ -223,3 +220,11 @@ def pc_diff(pc0,pc1,tol=0.1): rotation: ({rx,ry,rz}) """) + +# %% + + +s = np.load('/mount/wp/GEMstack/data/lidar1.npz')['arr_0'] # load null scene -- no added objects +s = s[~np.all(s == 0, axis=1)] +vis().add_pc(s,color='blue').show() +# %% diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py b/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py new file mode 100644 index 000000000..c09e64b49 --- /dev/null +++ b/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py @@ -0,0 +1,227 @@ +#%% +import os +import sys +import math +import numpy as np +from scipy.spatial.transform import Rotation as R +os.getcwd() +VIS = False + +#%% things to extract +tx,ty,tz,rx,ry,rz = [None] * 6 + +#%%============================================================== +#======================= util functions ========================= +#================================================================ +import pyvista as pv +import panel as pn +import matplotlib.pyplot as plt +def vis(ratio = 1): + pv.set_jupyter_backend('client') + plotter = pv.Plotter(notebook=True) + plotter.camera.position = (-20*ratio,0,20*ratio) + plotter.camera.focal_point = (0,0,0) + plotter.show_axes() + class foo: + def add_pc(self,pc,ratio=ratio,**kargs): + plotter.add_mesh( + pv.PolyData(pc*ratio), + render_points_as_spheres=True, + point_size=2, + **kargs) + return self + def add_line(self,p1,p2,ratio=ratio): + plotter.add_mesh( + pv.Line(p1*ratio,p2*ratio), + color='red', + line_width=1) + return self + def add_box(self,bound,trans,ratio=ratio): + l,w,h = map(lambda x:x*ratio,bound) + box = pv.Box(bounds=(-l/2,l/2,-w/2,w/2,-h/2,h/2)) + box = box.translate(list(map(lambda x:x*ratio,trans))) + plotter.add_mesh(box, color='yellow',show_edges=True) + return self + def show(self): + plotter.show() + return self + + return foo() +def load_scene(path): + sc = np.load(path)['arr_0'] + sc = sc[~np.all(sc == 0, axis=1)] # remove (0,0,0)'s + return sc +def crop(pc,ix=None,iy=None,iz=None): + # crop a subrectangle in a pointcloud + # usage: crop(pc, ix = (x_min,x_max), ...) + # return: array(N,3) + mask = True + for dim,intervel in zip([0,1,2],[ix,iy,iz]): + if not intervel: continue + d,u = intervel + mask &= pc[:,dim] >= d + mask &= pc[:,dim] <= u + print(f'points left after cropping {dim}\'th dim',mask.sum()) + return pc[mask] + +from sklearn.linear_model import RANSACRegressor +from sklearn.linear_model import LinearRegression +def fit_plane_ransac(pc,tol=0.01): + # fit a line/plane/hyperplane in a pointcloud + # pc: np array (N,D). the pointcloud + # tol: the tolerance. default 0.01 + model = RANSACRegressor(LinearRegression(), residual_threshold=tol) + model.fit(pc[:,:-1], pc[:,-1]) + a = model.estimator_.coef_ + inter = model.estimator_.intercept_ + class foo: + def plot(self): + inliers = pc[model.inlier_mask_] + if pc.shape[1] == 2: + plt.scatter(pc[:,0], pc[:,1], color='blue', marker='o', s=1) + plt.scatter(inliers[:,0], inliers[:,1], color='red', marker='o', s=1) + x_line = np.linspace(0, max(pc[:,0]), 100).reshape(-1,1) + plt.plot(x_line, x_line * a[0] + inter, color='red', label='Fitted Line') + elif pc.shape[1] == 3: + vis().add_pc(pc).add_pc(inliers,color='red').show() + return self + + def results(self): + # return: array(D-1), float, array(N,3) + # ^: , coeffs, intercept toward the plane, inliers of the fit + return a,inter + return foo() + +from scipy.spatial import cKDTree +def pc_diff(pc0,pc1,tol=0.1): + # remove points in pc0 from pc1 s.t. euclidian distance is within tolerance + # return: array(N,3) + tree = cKDTree(pc0) + diff = [] + for i, point in enumerate(pc1): + _, idx = tree.query(point) + distance = np.linalg.norm(point - pc0[idx]) # Compute the distance + if distance > tol: + diff.append(point) + tree = cKDTree(pc1) + for i, point in enumerate(pc0): + _, idx = tree.query(point) + distance = np.linalg.norm(point - pc1[idx]) # Compute the distance + if distance > tol: + diff.append(point) + return np.array(diff) + + + + + + +#%%============================================================== +#========================= tz rx ry ============================= +#================================================================ + +#%% load scene for ground plane +sc = load_scene('/mount/wp/GEMstack/data/lidar1.npz') + +# %% we crop to keep the ground +cropped_sc = crop(sc,iz = (-3,-2)) +if VIS: + print(cropped_sc.shape) + vis().add_pc(cropped_sc,color='blue').show() + +#%% +from math import sqrt +fit = fit_plane_ransac(cropped_sc,tol=0.01) +c, inter = fit.results() +normv = np.array([c[0], c[1], -1]) +normv /= np.linalg.norm(normv) +nx,ny,nz = normv +height = nz * inter +# TODO MAGIC NUMBER WARNING +height_axel = 0.2794 # 11 inches that we measured +tz = height - height_axel +if VIS: + fit.plot() +ry = math.atan2(nx, sqrt(ny**2 + nz**2)) +rx = math.atan2(-ny,sqrt(nx**2 + nz**2)) + + + +#%%============================================================== +#========================== tx ty rz ============================ +#================================================================ + +if False: # True to use the diff method to extract object. + # load data + sc0 = load_scene('/mount/wp/GEMstack/data/lidar70.npz') + sc1 = load_scene('/mount/wp/GEMstack/data/lidar78.npz') + + # crop to only keep a frontal box area + area = (-0,7),(-1,1),(-3,1) + cropped0 = crop(sc0,*area) + cropped1 = crop(sc1,*area) + print(cropped0.shape) + print(cropped1.shape) + + # Take difference to only keep added object + objects = pc_diff(cropped0,cropped1) +else: #False to use only cropping + sc1 = load_scene('/mount/wp/GEMstack/data/lidar1.npz') + + rot = R.from_euler('xyz',[rx,ry,0]).as_matrix() + + objects = sc1 @ rot.T + [0,0,tz] + + # crop to only keep a frontal box area + area = (-0,20),(-1,1),(0,1) + objects = crop(objects,*area) + print(objects.shape) + + +#%% +if VIS: + vis().add_pc(objects,color='blue').show() #visualize diff, hopefully the added objects + +# %% use the added objects to find rz. +# TODO after dataset retake +# right now we assume tx = ty = 0 and \ +# just use median to find a headding direction +fit = fit_plane_ransac(objects[:,:2],tol=0.6) +c,inter = fit.results() +if VIS: + fit.plot() +# tx = ty = 0 +# hx,hy = np.median(diff,axis=0)[:2] +# rz = -np.arctan2(hy,hx) +tx = - (2.56 - 1.46) # https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ +ty = - inter +rz = - math.atan(c) + +if VIS: + from scipy.spatial.transform import Rotation as R + rot = R.from_euler('xyz',[0,0,rz]).as_matrix() + cal_sc1 = sc1 @ rot.T + [tx,ty,0] + line = np.arange(0,100)/100*max(objects[:,0]) + line = np.stack((line,c[0]*line+inter,np.zeros(100)),axis=1) + line = line @ rot.T + [tx,ty,0] + vis().add_pc(cal_sc1,color='blue').add_pc(line,color='red').show() + + +#%% visualize calibrated pointcloud +if VIS: + rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() + + cal_sc1 = sc1 @ rot.T + [tx,ty,tz] + # projection + # cal_sc1[:,2] = 0 + v = vis(ratio=100) + v.add_pc(cal_sc1,color='blue') + v.add_box((2.56,.61*2,2.03+height_axel),[2.56/2,0,(2.03+height_axel)/2]) + v.show() +# %% +print(f""" +translation: ({tx,ty,tz}) +rotation: ({rx,ry,rz}) +""") + + From 124a75b8e38009548753c17e4cd3ea5bdf239862 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Wed, 12 Feb 2025 00:42:17 +0000 Subject: [PATCH 40/71] good visuals --- .../calibration/make_gem_e4_ouster_v2.py | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py b/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py index c09e64b49..948b17edf 100644 --- a/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py +++ b/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py @@ -5,7 +5,7 @@ import numpy as np from scipy.spatial.transform import Rotation as R os.getcwd() -VIS = False +VIS = True #%% things to extract tx,ty,tz,rx,ry,rz = [None] * 6 @@ -16,7 +16,8 @@ import pyvista as pv import panel as pn import matplotlib.pyplot as plt -def vis(ratio = 1): +def vis(title='', ratio=1): + print(title) pv.set_jupyter_backend('client') plotter = pv.Plotter(notebook=True) plotter.camera.position = (-20*ratio,0,20*ratio) @@ -40,7 +41,7 @@ def add_box(self,bound,trans,ratio=ratio): l,w,h = map(lambda x:x*ratio,bound) box = pv.Box(bounds=(-l/2,l/2,-w/2,w/2,-h/2,h/2)) box = box.translate(list(map(lambda x:x*ratio,trans))) - plotter.add_mesh(box, color='yellow',show_edges=True) + plotter.add_mesh(box, color='yellow') return self def show(self): plotter.show() @@ -66,7 +67,7 @@ def crop(pc,ix=None,iy=None,iz=None): from sklearn.linear_model import RANSACRegressor from sklearn.linear_model import LinearRegression -def fit_plane_ransac(pc,tol=0.01): +def fit_plane_ransac(pc,tol=0.01): # fit a line/plane/hyperplane in a pointcloud # pc: np array (N,D). the pointcloud # tol: the tolerance. default 0.01 @@ -78,12 +79,13 @@ class foo: def plot(self): inliers = pc[model.inlier_mask_] if pc.shape[1] == 2: + plt.title('ransac fitting line') plt.scatter(pc[:,0], pc[:,1], color='blue', marker='o', s=1) plt.scatter(inliers[:,0], inliers[:,1], color='red', marker='o', s=1) x_line = np.linspace(0, max(pc[:,0]), 100).reshape(-1,1) plt.plot(x_line, x_line * a[0] + inter, color='red', label='Fitted Line') elif pc.shape[1] == 3: - vis().add_pc(pc).add_pc(inliers,color='red').show() + vis('ransac fitting a plane').add_pc(pc).add_pc(inliers,color='red').show() return self def results(self): @@ -126,12 +128,11 @@ def pc_diff(pc0,pc1,tol=0.1): # %% we crop to keep the ground cropped_sc = crop(sc,iz = (-3,-2)) if VIS: - print(cropped_sc.shape) - vis().add_pc(cropped_sc,color='blue').show() + vis('ground points cropped').add_pc(cropped_sc,color='blue').show() #%% from math import sqrt -fit = fit_plane_ransac(cropped_sc,tol=0.01) +fit = fit_plane_ransac(cropped_sc,tol=0.01) # small tol because the ground is very flat c, inter = fit.results() normv = np.array([c[0], c[1], -1]) normv /= np.linalg.norm(normv) @@ -186,7 +187,7 @@ def pc_diff(pc0,pc1,tol=0.1): # TODO after dataset retake # right now we assume tx = ty = 0 and \ # just use median to find a headding direction -fit = fit_plane_ransac(objects[:,:2],tol=0.6) +fit = fit_plane_ransac(objects[:,:2],tol=1) # tol=1 because 1m^3 foam boxes c,inter = fit.results() if VIS: fit.plot() @@ -217,7 +218,9 @@ def pc_diff(pc0,pc1,tol=0.1): v = vis(ratio=100) v.add_pc(cal_sc1,color='blue') v.add_box((2.56,.61*2,2.03+height_axel),[2.56/2,0,(2.03+height_axel)/2]) - v.show() + v.show() + # the yellow box should be 11 inches above the ground + # rear-axel center should be at (0,0,0) # %% print(f""" translation: ({tx,ty,tz}) From 34f89eb5a831a32644034da1840fb38db82db647 Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 12 Feb 2025 14:13:49 -0600 Subject: [PATCH 41/71] Found a bug in longitutdinal_planning which used deceleration instead of acceleration when accelerating to max speed. Add velocities to trajectories so we can graph velocities, I'm not whether this is advisible. Added velocity graphs to test_longitudinal_plan for debugging. --- .../onboard/planning/longitudinal_planning.py | 32 +++-- GEMstack/state/trajectory.py | 1 + testing/test_longitudinal_plan.py | 129 ++++++++++++++++++ 3 files changed, 154 insertions(+), 8 deletions(-) create mode 100644 testing/test_longitudinal_plan.py diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 7651710b6..fad1f2875 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -41,12 +41,15 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m new_points = [] new_times = [] + velocities = [] # for graphing and debugging purposes + while current_speed > 0 or cur_index == 0: # we want to iterate through all the points and add them # to the new points. However, we also want to add "critical points" # where we reach top speed, begin decelerating, and stop new_points.append(cur_point) new_times.append(cur_time) + velocities.append(current_speed) print("=====================================") print("new points: ", new_points) print("current index: ", cur_index) @@ -115,7 +118,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) cur_time += delta_t_to_next_x cur_point = [next_point[0], 0] - current_speed += delta_t_to_next_x*deceleration + current_speed += delta_t_to_next_x*acceleration cur_index += 1 # this is the case where we would reach max speed before the next point @@ -182,19 +185,25 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m new_points.append(cur_point) new_times.append(cur_time) + velocities.append(current_speed) points = new_points times = new_times print("[PLAN] Computed points:", points) print("[TIME] Computed time:", times) + print("[Velocities] Computed velocities:", velocities) + #============================================= - trajectory = Trajectory(path.frame,points,times) + trajectory = Trajectory(path.frame,points,times,velocities) return trajectory def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: """Computes the time to go from x0 to x1 with initial velocity v0 and final velocity v1 - with constant acceleration a.""" + with constant acceleration a. I am assuming that we will always have a solution by settings + discriminant equal to zero, i'm not sure if this is an issue.""" + + """Consider changing the system to use linear operators instead of explicitly calculating because of instances here""" # x1 = x0 + v0*t + 0.5*a*t^2 # x1 - x0 = v0*t + 0.5*a*t^2 # 0.5*a*t^2 + v0*t + x0 - x1 = 0 @@ -205,14 +214,18 @@ def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: print("v: ", v) print("a: ", a) - t1 = (-v + (v**2 - 2*a*(x0-x1))**0.5)/a - t2 = (-v - (v**2 - 2*a*(x0-x1))**0.5)/a - if math.isnan(t1): t1 = 0 - if math.isnan(t2): t2 = 0 + t1 = (-v + max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a + t2 = (-v - max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a print("t1: ", t1) print("t2: ", t2) + if math.isnan(t1): t1 = 0 + if math.isnan(t2): t2 = 0 + + # print("t1: ", t1) + # print("t2: ", t2) + valid_times = [n for n in [t1, t2] if n > 0] if valid_times: return min(valid_times) @@ -238,18 +251,21 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) x_stop = x0 + current_speed * t_stop - 0.5 * deceleration * t_stop**2 new_points = [] + velocities = [] + for t in times: if t <= t_stop: x = x0 + current_speed * t - 0.5 * deceleration * t**2 else: x = x_stop new_points.append([x, 0]) + velocities.append(current_speed - deceleration * t) points = new_points print("[BRAKE] Computed points:", points) #============================================= - trajectory = Trajectory(path.frame,points,times) + trajectory = Trajectory(path.frame,points,times,velocities) return trajectory diff --git a/GEMstack/state/trajectory.py b/GEMstack/state/trajectory.py index d7a57db00..45e90b214 100644 --- a/GEMstack/state/trajectory.py +++ b/GEMstack/state/trajectory.py @@ -169,6 +169,7 @@ def trim(self, start : float, end : float) -> Path: class Trajectory(Path): """A timed, piecewise linear path.""" times : List[float] + velocities : Optional[List[float]] = None def domain(self) -> Tuple[float,float]: """Returns the time parameter domain""" diff --git a/testing/test_longitudinal_plan.py b/testing/test_longitudinal_plan.py new file mode 100644 index 000000000..481da2226 --- /dev/null +++ b/testing/test_longitudinal_plan.py @@ -0,0 +1,129 @@ +#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() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Braking from 0 m/s (should just stay still)") + plt.xlabel('time') + plt.ylabel('velocity') + 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() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + 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('velocity') + 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() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Accelerating from 0 m/s") + plt.xlabel('time') + plt.ylabel('velocity') + 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() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Accelerating from 2 m/s") + plt.xlabel('time') + plt.ylabel('velocity') + 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() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Keeping constant velocity at 3.1 m/s") + plt.xlabel('time') + plt.ylabel('velocity') + 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() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Too little time to stop, starting at 10 m/s") + plt.xlabel('time') + plt.ylabel('velocity') + 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() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Too little time to stop, braking at 10 m/s") + plt.xlabel('time') + plt.ylabel('velocity') + 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() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Nonuniform planning") + plt.xlabel('time') + plt.ylabel('velocity') + plt.show() + + +if __name__ == '__main__': + test_longitudinal_planning() From c914a41652f124eed4bfbd1755168b9f9a5331fc Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Thu, 13 Feb 2025 07:29:50 +0000 Subject: [PATCH 42/71] Little twick for rx,ry calculation --- .../calibration/make_gem_e4_ouster_v2.py | 47 ++++++++++++------- 1 file changed, 30 insertions(+), 17 deletions(-) diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py b/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py index 948b17edf..6e0b25940 100644 --- a/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py +++ b/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py @@ -5,7 +5,7 @@ import numpy as np from scipy.spatial.transform import Rotation as R os.getcwd() -VIS = True +VIS = False # True to show visuals #%% things to extract tx,ty,tz,rx,ry,rz = [None] * 6 @@ -13,9 +13,9 @@ #%%============================================================== #======================= util functions ========================= #================================================================ -import pyvista as pv -import panel as pn -import matplotlib.pyplot as plt +if VIS: + import pyvista as pv + import matplotlib.pyplot as plt def vis(title='', ratio=1): print(title) pv.set_jupyter_backend('client') @@ -31,10 +31,10 @@ def add_pc(self,pc,ratio=ratio,**kargs): point_size=2, **kargs) return self - def add_line(self,p1,p2,ratio=ratio): + def add_line(self,p1,p2,ratio=ratio,**kargs): plotter.add_mesh( pv.Line(p1*ratio,p2*ratio), - color='red', + **kargs, line_width=1) return self def add_box(self,bound,trans,ratio=ratio): @@ -123,7 +123,7 @@ def pc_diff(pc0,pc1,tol=0.1): #================================================================ #%% load scene for ground plane -sc = load_scene('/mount/wp/GEMstack/data/lidar1.npz') +sc = load_scene('/mount/wp/GEMstack/data/lidar70.npz') # %% we crop to keep the ground cropped_sc = crop(sc,iz = (-3,-2)) @@ -143,20 +143,31 @@ def pc_diff(pc0,pc1,tol=0.1): tz = height - height_axel if VIS: fit.plot() -ry = math.atan2(nx, sqrt(ny**2 + nz**2)) -rx = math.atan2(-ny,sqrt(nx**2 + nz**2)) +rx = -math.atan2(ny,-nz) +ry = -math.atan2(-nx,-nz) +if VIS: + from scipy.spatial.transform import Rotation as R + rot = R.from_euler('xyz',[rx,ry,0]).as_matrix() + cal_sc = sc @ rot.T + [0,0,tz] + vis('yz projection').add_pc(cal_sc*[0,1,1],color='blue').show() + vis('xz projection').add_pc(cal_sc*[1,0,1],color='blue').show() #%%============================================================== #========================== tx ty rz ============================ #================================================================ +rot = R.from_euler('xyz',[rx,ry,0]).as_matrix() + if False: # True to use the diff method to extract object. # load data sc0 = load_scene('/mount/wp/GEMstack/data/lidar70.npz') sc1 = load_scene('/mount/wp/GEMstack/data/lidar78.npz') + sc0 = sc0 @ rot.T + [0,0,tz] + sc1 = sc1 @ rot.T + [0,0,tz] + # crop to only keep a frontal box area area = (-0,7),(-1,1),(-3,1) cropped0 = crop(sc0,*area) @@ -166,11 +177,10 @@ def pc_diff(pc0,pc1,tol=0.1): # Take difference to only keep added object objects = pc_diff(cropped0,cropped1) + else: #False to use only cropping sc1 = load_scene('/mount/wp/GEMstack/data/lidar1.npz') - rot = R.from_euler('xyz',[rx,ry,0]).as_matrix() - objects = sc1 @ rot.T + [0,0,tz] # crop to only keep a frontal box area @@ -199,13 +209,14 @@ def pc_diff(pc0,pc1,tol=0.1): rz = - math.atan(c) if VIS: + p1 = (0,inter,0) + p2 = max(objects[:,0])*np.array([1,c[0],0])+np.array([0,inter,0]) + vis().add_pc(sc1*np.array([1,1,0]),color='blue').add_line(p1,p2,color='red').show() + from scipy.spatial.transform import Rotation as R rot = R.from_euler('xyz',[0,0,rz]).as_matrix() cal_sc1 = sc1 @ rot.T + [tx,ty,0] - line = np.arange(0,100)/100*max(objects[:,0]) - line = np.stack((line,c[0]*line+inter,np.zeros(100)),axis=1) - line = line @ rot.T + [tx,ty,0] - vis().add_pc(cal_sc1,color='blue').add_pc(line,color='red').show() + vis().add_pc(cal_sc1,color='blue').show() #%% visualize calibrated pointcloud @@ -214,9 +225,9 @@ def pc_diff(pc0,pc1,tol=0.1): cal_sc1 = sc1 @ rot.T + [tx,ty,tz] # projection - # cal_sc1[:,2] = 0 + # cal_sc1[:,1] = 0 v = vis(ratio=100) - v.add_pc(cal_sc1,color='blue') + v.add_pc(cal_sc1*[0,1,1],color='blue') v.add_box((2.56,.61*2,2.03+height_axel),[2.56/2,0,(2.03+height_axel)/2]) v.show() # the yellow box should be 11 inches above the ground @@ -228,3 +239,5 @@ def pc_diff(pc0,pc1,tol=0.1): """) + +# %% From ba645ede721c147e5333c547de6b76cbe460f996 Mon Sep 17 00:00:00 2001 From: Sai Rohit Muralikrishnan Date: Thu, 13 Feb 2025 03:03:29 -0600 Subject: [PATCH 43/71] Your commit message --- .../onboard/planning/longitudinal_planning.py | 85 +++++++++++++++++-- 1 file changed, 76 insertions(+), 9 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 68f04e92a..417cf07a8 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -25,6 +25,11 @@ def solve_for_v_peak(v0: float, acceleration: float, deceleration: float, total_ return math.sqrt(v_peak_sq) +def compute_dynamic_dt(acceleration, speed, k=0.02, a_min=0.5): + position_step = k * max(speed, 1.0) # Ensures position step is speed-dependent + return np.sqrt(2 * position_step / max(acceleration, a_min)) + + def longitudinal_plan(path, acceleration: float, deceleration: float, max_speed: float, current_speed: float): @@ -59,13 +64,14 @@ def longitudinal_plan(path, acceleration: float, deceleration: float, max_speed: t_final = t_accel + t_decel profile_type = "triangular" - # 4. Create a time grid. - dt = 0.1 # adjust based on computation - times = np.arange(0, t_final + dt, dt) - - # 5. Compute the distance s(t) for each time step. + t = 0 + times = [] s_vals = [] - for t in times: + num_time_steps = 0 + while t < t_final: + dt = compute_dynamic_dt(acceleration if t < t_accel else deceleration,current_speed) + t = t + dt + times.append(t) if profile_type == "trapezoidal": if t < t_accel: # Acceleration phase. @@ -76,7 +82,6 @@ def longitudinal_plan(path, acceleration: float, deceleration: float, max_speed: else: # Deceleration phase. t_decel_phase = t - (t_accel + t_cruise) - # Compute the remaining distance using the deceleration equation. s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase)**2 else: # Triangular profile. if t < t_accel: @@ -87,11 +92,59 @@ def longitudinal_plan(path, acceleration: float, deceleration: float, max_speed: s_accel = current_speed * t_accel + 0.5 * acceleration * t_accel**2 s = s_accel + peak_speed * t_decel_phase - 0.5 * deceleration * t_decel_phase**2 - # should not exceed total path length s_vals.append(min(s, total_length)) + if s >= total_length: + break + num_time_steps +=1 - + # Compute trajectory points points = [path_norm.eval(s) for s in s_vals] + print("Number of time steps is --------------------", num_time_steps) + + return Trajectory(path_norm.frame, points, times) + + + + + + + # 4. Create a time grid. + # dt = 0.1 # adjust based on computation + # times = np.arange(0, t_final + dt, dt) + # num_time_steps = 0 + + # # 5. Compute the distance s(t) for each time step. + # s_vals = [] + # for t in times: + # if profile_type == "trapezoidal": + # if t < t_accel: + # # Acceleration phase. + # s = current_speed * t + 0.5 * acceleration * t**2 + # elif t < t_accel + t_cruise: + # # Cruise phase. + # s = d_accel + max_speed * (t - t_accel) + # else: + # # Deceleration phase. + # t_decel_phase = t - (t_accel + t_cruise) + # # Compute the remaining distance using the deceleration equation. + # s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase)**2 + # else: # Triangular profile. + # if t < t_accel: + # # Acceleration phase. + # s = current_speed * t + 0.5 * acceleration * t**2 + # else: + # t_decel_phase = t - t_accel + # s_accel = current_speed * t_accel + 0.5 * acceleration * t_accel**2 + # s = s_accel + peak_speed * t_decel_phase - 0.5 * deceleration * t_decel_phase**2 + # num_time_steps +=1 + + # # should not exceed total path length + # s_vals.append(min(s, total_length)) + # print("NUmber of time steps -----------",num_time_steps) + # print("T FInal ----------------------------", t_final) + # points = [path_norm.eval(s) for s in s_vals] + + trajectory = Trajectory(path_norm.frame, points, list(times)) return trajectory @@ -103,9 +156,23 @@ def longitudinal_plan(path, acceleration: float, deceleration: float, max_speed: 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() + # print("paaaaaaa", path_normalized) #TODO: actually do something to points and times points = [p for p in path_normalized.points] times = [t for t in path_normalized.times] + # print("points",points) + x0 = points[0][0] + t_stop = current_speed/deceleration + x_stop = x0 + current_speed * t_stop - 0.5 * deceleration * t_stop**2 + new_points = [] + for t in times: + if t <= t_stop: + x = x0 + current_speed * t - 0.5 * deceleration * t**2 + else: + x = x_stop + new_points.append([x, 0]) + points = new_points + trajectory = Trajectory(path.frame,points,times) return trajectory From d2235bcd77e0b55e04c4dd0a109bbce47d83404b Mon Sep 17 00:00:00 2001 From: Sai Rohit Muralikrishnan Date: Thu, 13 Feb 2025 03:20:32 -0600 Subject: [PATCH 44/71] after dynamic dt --- GEMstack/onboard/planning/longitudinal_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 417cf07a8..382931be5 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -101,7 +101,7 @@ def longitudinal_plan(path, acceleration: float, deceleration: float, max_speed: points = [path_norm.eval(s) for s in s_vals] print("Number of time steps is --------------------", num_time_steps) - return Trajectory(path_norm.frame, points, times) + # return Trajectory(path_norm.frame, points, times) From 299768a233d9d4d332e0086b3dd5f1cea913d4d3 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Thu, 13 Feb 2025 11:35:44 -0600 Subject: [PATCH 45/71] Fixed aborting at the end of sim. Modified k value for smoother accel in sim. Modified output values to include initial values. Made real time plot and comment out. --- .../onboard/planning/longitudinal_planning.py | 27 ++++++++++++++++--- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 382931be5..ca2f96f4a 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -25,7 +25,7 @@ def solve_for_v_peak(v0: float, acceleration: float, deceleration: float, total_ return math.sqrt(v_peak_sq) -def compute_dynamic_dt(acceleration, speed, k=0.02, a_min=0.5): +def compute_dynamic_dt(acceleration, speed, k=0.005, a_min=0.5): position_step = k * max(speed, 1.0) # Ensures position step is speed-dependent return np.sqrt(2 * position_step / max(acceleration, a_min)) @@ -38,6 +38,14 @@ def longitudinal_plan(path, acceleration: float, deceleration: float, max_speed: path_norm = path.arc_length_parameterize(speed=1.0) total_length = path.length() + # ------------------- + # If the path is too short, just return the path for preventing sudden halt of simulation + if total_length < 0.05: + points = [p for p in path_norm.points] + times = [t for t in path_norm.times] + return Trajectory(path.frame, points, times) + # ------------------- + # 2. Compute distances for d_accel,d_decel if max_speed > current_speed: d_accel = (max_speed**2 - current_speed**2) / (2 * acceleration) @@ -69,8 +77,6 @@ def longitudinal_plan(path, acceleration: float, deceleration: float, max_speed: s_vals = [] num_time_steps = 0 while t < t_final: - dt = compute_dynamic_dt(acceleration if t < t_accel else deceleration,current_speed) - t = t + dt times.append(t) if profile_type == "trapezoidal": if t < t_accel: @@ -95,6 +101,10 @@ def longitudinal_plan(path, acceleration: float, deceleration: float, max_speed: s_vals.append(min(s, total_length)) if s >= total_length: break + + dt = compute_dynamic_dt(acceleration if t < t_accel else deceleration,current_speed) + t = t + dt + num_time_steps +=1 # Compute trajectory points @@ -104,7 +114,16 @@ def longitudinal_plan(path, acceleration: float, deceleration: float, max_speed: # return Trajectory(path_norm.frame, points, times) - + # # Plot: update a single window + # import matplotlib.pyplot as plt + # plt.figure("Distance vs Time") + # plt.clf() # Clear the current figure + # plt.plot(times, s_vals) + # plt.xlabel("Time (s)") + # plt.ylabel("Distance (m)") + # plt.title("Distance vs Time") + # plt.draw() + # plt.pause(0.001) From 03f46c1929c7800e1e9f37f7091da58ed9bbadfd Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Wed, 12 Feb 2025 19:10:06 -0600 Subject: [PATCH 46/71] Fix jitter at the end of track for simulation --- GEMstack/onboard/interface/gem_simulator.py | 2 +- GEMstack/onboard/planning/pure_pursuit.py | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_simulator.py b/GEMstack/onboard/interface/gem_simulator.py index 2abfde71f..d1574e7f7 100644 --- a/GEMstack/onboard/interface/gem_simulator.py +++ b/GEMstack/onboard/interface/gem_simulator.py @@ -207,7 +207,7 @@ def simulate(self, T : float, command : Optional[GEMVehicleCommand]): reading.brake_pedal_position = brake_pedal_position reading.accelerator_pedal_position = 0 reading.speed = v - if v > 0: + if v >= 0: reading.gear = 1 else: reading.gear = -1 diff --git a/GEMstack/onboard/planning/pure_pursuit.py b/GEMstack/onboard/planning/pure_pursuit.py index 48c0b7dfd..d3461eb02 100644 --- a/GEMstack/onboard/planning/pure_pursuit.py +++ b/GEMstack/onboard/planning/pure_pursuit.py @@ -189,6 +189,11 @@ def compute(self, state : VehicleState, component : Component = None): output_accel = -self.max_decel self.t_last = t + + if desired_speed == 0 and speed == 0 and output_accel < 0.0: + print("Stopping. Set accel", output_accel, "to 0") + output_accel = 0.0 + return (output_accel, f_delta) From bd94404eec4d3777f4bc98e22f9e325665a8aa7c Mon Sep 17 00:00:00 2001 From: Averyyy <58547284+Averyyy@users.noreply.github.com> Date: Thu, 13 Feb 2025 18:06:32 -0600 Subject: [PATCH 47/71] feat: remove ibeo msgs in setup_this machine.sh --- setup/setup_this_machine.sh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/setup/setup_this_machine.sh b/setup/setup_this_machine.sh index e30b3aa23..202aa1be7 100755 --- a/setup/setup_this_machine.sh +++ b/setup/setup_this_machine.sh @@ -20,6 +20,9 @@ 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 .. +# Remove the ibeo_msgs folder so it is not included +rm -rf astuff_sensor_msgs/ibeo_msgs + cd .. #back to catkin_ws rosdep install --from-paths src --ignore-src -r -y catkin_make -DCMAKE_BUILD_TYPE=Release From 7dd38e301e9a5408923b1f6652dbc65364440897 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Fri, 14 Feb 2025 00:16:46 -0600 Subject: [PATCH 48/71] Modified braking function for experiment --- .../onboard/planning/longitudinal_planning.py | 14 ++++++----- launch/launch_all.sh | 24 +++++++++++++++++++ launch/pedestrian_detection.yaml | 6 ++--- 3 files changed, 35 insertions(+), 9 deletions(-) create mode 100644 launch/launch_all.sh diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index fad1f2875..d4550d708 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -350,11 +350,13 @@ def update(self, state : AllState): print("ped", a.pose.x, a.pose.y) print("ego", abs_x, abs_y) - # TODO: Make logic for smooth deceleration and re-acceleration - # TEMPORARY: STOP WHEN WITHIN 10M OF PEDESTRIAN - if a.pose.x - abs_x < 10.0 and a.pose.x - abs_x > 0.0: - print("#### Yielding to",n) - should_brake = True + should_brake = True + + # # TODO: Make logic for smooth deceleration and re-acceleration + # # TEMPORARY: STOP WHEN WITHIN 10M OF PEDESTRIAN + # if a.pose.x - abs_x < 10.0 and a.pose.x - abs_x > 0.0: + # print("#### Yielding to",n) + # should_brake = True break @@ -362,7 +364,7 @@ def update(self, state : AllState): # should_brake = True # # UNCOMMENT NOT TO BRAKE - should_brake = False + # should_brake = False #========================= diff --git a/launch/launch_all.sh b/launch/launch_all.sh new file mode 100644 index 000000000..c75fbecc9 --- /dev/null +++ b/launch/launch_all.sh @@ -0,0 +1,24 @@ +#!/bin/bash +<< COMMENTOUT +########## +- This script opens three new tabs in GNOME Terminal. +- Each tab will change to the GEMstack directory, source the ROS environment, and then run the specified command. + +- Run these commands at each tab: +cd ~/catkin_ws/src/GEMstack +source ~/catkin_ws/devel/setup.bash && roslaunch basic_launch sensor_init.launch +source ~/catkin_ws/devel/setup.bash && roslaunch basic_launch dbw_joystick.launch +source ~/catkin_ws/devel/setup.bash && python3 main.py --variant=fake_real launch/pedestrian_detection.yaml + +- If you don't want to detect virtual pedestrian, run this command instead: +source ~/catkin_ws/devel/setup.bash && python3 main.py launch/pedestrian_detection.yaml + +- If roscore cannot run, run this command to kill all roscore: +killall -9 roscore rosmaster +########## +COMMENTOUT + +cd ~/catkin_ws/src/GEMstack +gnome-terminal --tab -- bash -c "source ~/catkin_ws/devel/setup.bash && roslaunch basic_launch sensor_init.launch; exec bash" +gnome-terminal --tab -- bash -c "source ~/catkin_ws/devel/setup.bash && roslaunch basic_launch dbw_joystick.launch; exec bash" +gnome-terminal --tab -- bash -c "source ~/catkin_ws/devel/setup.bash && python3 main.py --variant=fake_real launch/pedestrian_detection.yaml; exec bash" \ No newline at end of file diff --git a/launch/pedestrian_detection.yaml b/launch/pedestrian_detection.yaml index 3c5cd9181..623bc682c 100644 --- a/launch/pedestrian_detection.yaml +++ b/launch/pedestrian_detection.yaml @@ -94,6 +94,6 @@ variants: visualization: !include "klampt_visualization.yaml" drive: perception: - state_estimation : OmniscientStateEstimator - agent_detection : OmniscientAgentDetector - + agent_detection : pedestrian_detection.FakePedestrianDetector2D + #agent_detection : OmniscientAgentDetector #this option reads agents from the simulator + state_estimation : OmniscientStateEstimator \ No newline at end of file From 0831c678e156264e015d653d2650b0e611a17a9f Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Fri, 14 Feb 2025 00:32:13 -0600 Subject: [PATCH 49/71] Implemented Part 1&2 --- .../onboard/planning/longitudinal_planning.py | 199 ++++++++++++++++-- launch/pedestrian_detection.yaml | 4 +- 2 files changed, 188 insertions(+), 15 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index e862239fe..1087bd831 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -1,9 +1,89 @@ -from typing import List +from typing import List, Tuple from ..component import Component -from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, ObjectFrameEnum +from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, ObjectFrameEnum, AgentState from ...utils import serialization from ...mathutils.transforms import vector_madd +import time +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.patches as patches +import math + +def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float) -> Tuple[bool, float]: + """Detects if a collision will occur with the given object and return deceleration to avoid it.""" + + # Get the object's position and velocity + obj_x = obj.pose.x + obj_y = obj.pose.y + obj_v_x = obj.velocity[0] + obj_v_y = obj.velocity[1] + + pedestrian_length = 0.5 + pedestrian_width = 0.5 + + vehicle_length = 3.5 + vehicle_width = 2 + + vehicle_buffer_x = 3.0 + vehicle_buffer_y = 1.5 + + vehicle_front = curr_x + vehicle_length / 2 + vehicle_back = curr_x - vehicle_length / 2 + vehicle_left = curr_y + vehicle_width / 2 + vehicle_right = curr_y - vehicle_width / 2 + + pedestrian_front = obj_x + pedestrian_length / 2 + pedestrian_back = obj_x - pedestrian_length / 2 + pedestrian_left = obj_y + pedestrian_width / 2 + pedestrian_right = obj_y - pedestrian_width / 2 + + # Check if the object is in front of the vehicle + if vehicle_front > pedestrian_back: + if vehicle_back > pedestrian_front: + # The object is behind the vehicle + print("Object is behind the vehicle") + return False, 0.0 + if vehicle_right - vehicle_buffer_y > pedestrian_left or vehicle_left + vehicle_buffer_y < pedestrian_right: + # The object is to the side of the vehicle + print("Object is to the side of the vehicle") + return False, 0.0 + # The object overlaps with the vehicle + return True, max_deceleration + + if vehicle_right - vehicle_buffer_y > pedestrian_left and obj_v_y <= 0: + # The object is to the right of the vehicle and moving away + print("Object is to the right of the vehicle and moving away") + return False, 0.0 + + if vehicle_left + vehicle_buffer_y < pedestrian_right and obj_v_y >= 0: + # The object is to the left of the vehicle and moving away + print("Object is to the left of the vehicle and moving away") + return False, 0.0 + + if vehicle_front + vehicle_buffer_x >= pedestrian_back: + # The object is in front of the vehicle and within the buffer + print("Object is in front of the vehicle and within the buffer") + return True, max_deceleration + + # Calculate the deceleration needed to avoid a collision + print("Object is in front of the vehicle and outside the buffer") + distance = pedestrian_back - vehicle_front - vehicle_buffer_x + + relative_v = curr_v - obj_v_x + if relative_v <= 0: + return False, 0.0 + + print(relative_v, distance) + + deceleration = relative_v ** 2 / (2 * distance) + if deceleration > max_deceleration: + return True, max_deceleration + if deceleration < min_deceleration: + return False, 0.0 + + return True, deceleration + 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. @@ -14,9 +94,43 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m 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] + + #============================================= + + print("-----LONGITUDINAL PLAN-----") + + look_ahead_time = 10.0 + dt = 0.2 + stop_distance_buffer = 0.2 + + trajectory_length = (look_ahead_time / dt) + 1 + times = np.linspace(0, look_ahead_time, int(trajectory_length)).tolist() + + new_points = [points[0]] + + for _ in range(1, len(times)): + distance_to_stop = current_speed **2 / (2 * deceleration) + if new_points[-1][0] >= points[-1][0]: + new_points.append([new_points[-1][0], 0]) + elif (distance_to_stop + stop_distance_buffer) >= (points[-1][0] - new_points[-1][0]): + # start decelerating + new_points.append([new_points[-1][0] + current_speed * dt - 0.5 * deceleration * dt**2, 0]) + current_speed -= deceleration * dt + elif current_speed < max_speed: + # start accelerating + new_points.append([new_points[-1][0] + current_speed * dt + 0.5 * acceleration * dt**2, 0]) + current_speed += acceleration * dt + else: + new_points.append([new_points[-1][0] + current_speed * dt, 0]) + + points = new_points + + for p, t in zip(points, times): + print(f"Time: {t:.2f}, Point: {p}") + trajectory = Trajectory(path.frame,points,times) return trajectory @@ -24,9 +138,39 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m 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] + + #============================================= + + print("=====LONGITUDINAL BRAKE=====") + print("deceleration: ", deceleration) + + look_ahead_time = 10.0 + dt = 0.2 + + print(times[0]) + + trajectory_length = (look_ahead_time / dt) + 1 + times = np.linspace(0, look_ahead_time, int(trajectory_length)).tolist() + + stop_time = current_speed / deceleration + stop_distance = current_speed * stop_time / 2 + + new_points = [points[0]] + for i in range(1, len(times)): + if times[i] < stop_time: + x = points[0][0] + current_speed * times[i] - 0.5 * deceleration * times[i]**2 + else: + x = points[0][0] + stop_distance + new_points.append([x, 0]) + points = new_points + + for p, t in zip(points, times): + print(f"Time: {t:.2f}, Point: {p}") + trajectory = Trajectory(path.frame,points,times) return trajectory @@ -36,6 +180,7 @@ class YieldTrajectoryPlanner(Component): 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 @@ -43,6 +188,9 @@ def __init__(self): self.desired_speed = 1.0 self.deceleration = 2.0 + self.min_deceleration = 1.0 + self.max_deceleration = 8.0 + def state_inputs(self): return ['all'] @@ -53,6 +201,8 @@ def rate(self): return 10.0 def update(self, state : AllState): + start_time = time.time() + vehicle = state.vehicle # type: VehicleState route = state.route # type: Route t = state.t @@ -61,33 +211,56 @@ def update(self, state : AllState): self.t_last = t dt = t - self.t_last + # Position in vehicle frame (Start (0,0) to (15,0)) curr_x = vehicle.pose.x curr_y = vehicle.pose.y curr_v = vehicle.v + abs_x = curr_x + state.start_vehicle_pose.x + abs_y = curr_y + state.start_vehicle_pose.y + #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 + lookahead_distance = max(10, curr_v**2 / (2 * self.deceleration)) + route_with_lookahead = route.trim(closest_parameter,closest_parameter + lookahead_distance) + print("Lookahead distance:", lookahead_distance) + + route_to_end = route.trim(closest_parameter, len(route.points) - 1) + #extract out a 10m segment of the route - route_with_lookahead = route.trim(closest_parameter,closest_parameter+10.0) + # route_with_lookahead = route.trim(closest_parameter,closest_parameter+10.0) + + should_yield = False + yield_deceleration = 0.0 + + print("Current Speed: ", curr_v) - #parse the relations indicated - should_brake = False for r in state.relations: if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': #yielding to something, brake - should_brake = True - should_accelerate = (not should_brake and curr_v < self.desired_speed) + + #========================= + # print("#### YIELDING PLANNING ####") + + #get the object we are yielding to + obj = state.agents[r.obj2] + + detected, deceleration = detect_collision(abs_x, abs_y, curr_v, obj, self.min_deceleration, self.max_deceleration) + + if detected and deceleration > 0: + yield_deceleration = max(deceleration, yield_deceleration) + should_yield = True + + print("should yield: ", should_yield) #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) + if should_yield: + traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) else: - traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) + traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v) return traj diff --git a/launch/pedestrian_detection.yaml b/launch/pedestrian_detection.yaml index 1a00fa797..98f49a9aa 100644 --- a/launch/pedestrian_detection.yaml +++ b/launch/pedestrian_detection.yaml @@ -94,6 +94,6 @@ variants: visualization: !include "klampt_visualization.yaml" drive: perception: - agent_detection : pedestrian_detection.FakePedestrianDetector2D - #agent_detection : OmniscientAgentDetector #this option reads agents from the simulator + # agent_detection : pedestrian_detection.FakePedestrianDetector2D + agent_detection : OmniscientAgentDetector #this option reads agents from the simulator state_estimation : OmniscientStateEstimator From 0f3827049dc965ba96890022e8dd0d8d6441609a Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Fri, 14 Feb 2025 10:55:06 +0000 Subject: [PATCH 50/71] manual cali lidar to camera --- .../calibration/lidar_to_camera_manual.py | 191 +++++++++++++++ .../calibration/make_gem_e4_ouster.py | 230 ------------------ 2 files changed, 191 insertions(+), 230 deletions(-) create mode 100644 GEMstack/offboard/calibration/lidar_to_camera_manual.py delete mode 100644 GEMstack/offboard/calibration/make_gem_e4_ouster.py diff --git a/GEMstack/offboard/calibration/lidar_to_camera_manual.py b/GEMstack/offboard/calibration/lidar_to_camera_manual.py new file mode 100644 index 000000000..8d25510dd --- /dev/null +++ b/GEMstack/offboard/calibration/lidar_to_camera_manual.py @@ -0,0 +1,191 @@ +#%% +import cv2 +from scipy.spatial.transform import Rotation as R +import matplotlib.pyplot as plt +import numpy as np + +rgb_path = '/mount/wp/GEMstack/data/color21.png' +depth_path = '/mount/wp/GEMstack/data/depth21.tif' +lidar_path = '/mount/wp/GEMstack/data/lidar21.npz' + +img = cv2.imread(rgb_path, cv2.IMREAD_UNCHANGED) + +lidar_points = np.load(lidar_path)['arr_0'] +lidar_points = lidar_points[~np.all(lidar_points== 0, axis=1)] # remove (0,0,0)'s + +rx,ry,rz = 0.006898647163954201, 0.023800082245145304, -0.025318355743942974 +tx,ty,tz = -1.1, 0.037735827433173136, 1.953202227766785 +rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() +lidar_ex = np.hstack([rot,[[tx],[ty],[tz]]]) +lidar_ex = np.vstack([lidar_ex,[0,0,0,1]]) + +camera_in = np.array([ + [684.83331299, 0. , 573.37109375], + [ 0. , 684.60968018, 363.70092773], + [ 0. , 0. , 1. ] +], dtype=np.float32) + +#%% +# blurred = cv2.GaussianBlur(img, (5, 5), 0) +# edges = cv2.Canny(blurred, threshold1=50, threshold2=150) +# plt.imshow(edges) +plt.imshow(cv2.cvtColor(img,cv2.COLOR_BGR2RGB)) + +#%% +import pyvista as pv +def vis(title='', ratio=1): + print(title) + pv.set_jupyter_backend('client') + plotter = pv.Plotter(notebook=True) + plotter.show_axes() + class foo: + def set_cam(self,pos=(-20*ratio,0,20*ratio),foc=(0,0,0)): + plotter.camera.position = pos + plotter.camera.focal_point = foc + return self + def add_pc(self,pc,ratio=ratio,**kargs): + plotter.add_mesh( + pv.PolyData(pc*ratio), + render_points_as_spheres=True, + point_size=2, + **kargs) + return self + def add_line(self,p1,p2,ratio=ratio,**kargs): + plotter.add_mesh( + pv.Line(p1*ratio,p2*ratio), + **kargs, + line_width=1) + return self + def add_box(self,bound,trans,ratio=ratio): + l,w,h = map(lambda x:x*ratio,bound) + box = pv.Box(bounds=(-l/2,l/2,-w/2,w/2,-h/2,h/2)) + box = box.translate(list(map(lambda x:x*ratio,trans))) + plotter.add_mesh(box, color='yellow') + return self + def show(self): + plotter.show() + return self + def close(self): + plotter.close() + return None + + return foo().set_cam() +def crop(pc,ix=None,iy=None,iz=None): + # crop a subrectangle in a pointcloud + # usage: crop(pc, ix = (x_min,x_max), ...) + # return: array(N,3) + mask = True + for dim,intervel in zip([0,1,2],[ix,iy,iz]): + if not intervel: continue + d,u = intervel + mask &= pc[:,dim] >= d + mask &= pc[:,dim] <= u + print(f'points left after cropping {dim}\'th dim',mask.sum()) + return pc[mask] + + +lidar_post = np.pad(lidar_points,((0,0),(0,1)),constant_values=1) @ lidar_ex.T +lidar_post = crop(lidar_post,ix=(0,8),iy=(-5,5)) +vis().add_pc(lidar_post).show() + +#%% +def pick_4_img(img): + corners = [] # Reset the corners list + def click_event(event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN: + corners.append((x, y)) + cv2.circle(param, (x, y), 5, (0, 255, 0), -1) + cv2.imshow('Image', param) + + cv2.imshow('Image', img) + cv2.setMouseCallback('Image', click_event, img) + + while True: + if len(corners) == 4: + break + if cv2.waitKey(1) & 0xFF == ord('q'): + return None + + cv2.destroyAllWindows() + + return corners +cpoints = np.array(pick_4_img(img)).astype(float) +print(cpoints) + +#%% +def pick_4_pc(point_cloud): + points = [] + def cb(pt,*args): + points.append(pt) + while len(points)!=4: + points = [] + cloud = pv.PolyData(point_cloud) + plotter = pv.Plotter(notebook=False) + plotter.camera.position = (-20,0,20) + plotter.camera.focal_point = (0,0,0) + plotter.add_mesh(cloud, color='lightblue', point_size=5, render_points_as_spheres=True) + plotter.enable_point_picking(cb) + plotter.show() + return points + +lpoints = np.array(pick_4_pc(lidar_post)) +print(lpoints) +# %% +success, rvec, tvec = cv2.solvePnP(lpoints, cpoints, camera_in, None) +R, _ = cv2.Rodrigues(rvec) + +T = np.eye(4) +T[:3, :3] = R +T[:3, 3] = tvec.flatten() +print(T) + + +#%% +def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): + """ + Convert a single-channel depth image into an Nx3 array of 3D points + in the camera coordinate system. + - depth_img: 2D array of type uint16 or float with depth values + - intrinsics: 3x3 camera matrix + """ + fx = intrinsics[0,0] + fy = intrinsics[1,1] + cx = intrinsics[0,2] + cy = intrinsics[1,2] + + # Indices of each pixel + h, w = depth_img.shape + i_range = np.arange(h) + j_range = np.arange(w) + jj, ii = np.meshgrid(j_range, i_range) # shape (h,w) + + # Flatten + ii = ii.flatten().astype(np.float32) + jj = jj.flatten().astype(np.float32) + depth = depth_img.flatten().astype(np.float32) + + # Filter out zero / invalid depths + valid_mask = (depth > 0) + ii = ii[valid_mask] + jj = jj[valid_mask] + depth = depth[valid_mask] + + z = depth / 10000 + x = (jj - cx) * z / fx + y = (ii - cy) * z / fy + points3d = np.stack((x, y, z), axis=-1) # shape (N,3) + + return points3d + + + +depth_img = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED) +camera_points = depth_to_points(depth_img, camera_in) + +v=vis() +v.add_pc(np.pad(lidar_points,((0,0),(0,1)),constant_values=1)@lidar_ex.T@T.T[:,:3],color='blue') +v.add_pc(camera_points,color='red') +v.show() + +#%% +print(np.vstack([(lidar_ex.T@T.T[:,:3]).T,[0,0,0,1]])) \ No newline at end of file diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster.py b/GEMstack/offboard/calibration/make_gem_e4_ouster.py deleted file mode 100644 index 3d5053740..000000000 --- a/GEMstack/offboard/calibration/make_gem_e4_ouster.py +++ /dev/null @@ -1,230 +0,0 @@ -#%% -import os -import sys -import math -os.getcwd() -VIS = False - -#%% things to extract -tx,ty,tz,rx,ry,rz = [None] * 6 -#%% -#TODO make into command line arguments -pbg = '/mount/wp/GEMstack/data/lidar70.npz' #null scene -pbgAndLine = '/mount/wp/GEMstack/data/lidar78.npz' - -#%% load and wash data -import numpy as np - -bg = np.load(pbg)['arr_0'] # load null scene -- no added objects -bgAndLine = np.load(pbgAndLine)['arr_0'] # load data pc -- with added object -bg = bg[~np.all(bg == 0, axis=1)] # remove (0,0,0)'s -bgAndLine = bgAndLine[~np.all(bgAndLine == 0, axis=1)] - -#%% visualize cropped null scene and data scene -import pyvista as pv -import panel as pn -import matplotlib.pyplot as plt -def vis(ratio = 1): - pv.set_jupyter_backend('client') - plotter = pv.Plotter(notebook=True) - plotter.camera.position = (-20*ratio,0,20*ratio) - plotter.camera.focal_point = (0,0,0) - plotter.show_axes() - class foo: - def add_pc(self,pc,ratio=ratio,**kargs): - print(ratio) - plotter.add_mesh( - pv.PolyData(pc*ratio), - render_points_as_spheres=True, - point_size=2, - **kargs) - return self - def add_line(self,p1,p2,ratio=ratio): - plotter.add_mesh( - pv.Line(p1*ratio,p2*ratio), - color='red', - line_width=1) - return self - def add_box(self,bound,trans,ratio=ratio): - print(ratio) - l,w,h = map(lambda x:x*ratio,bound) - box = pv.Box(bounds=(-l/2,l/2,-w/2,w/2,-h/2,h/2)) - print(box) - print(*map(lambda x:x*ratio,trans)) - box = box.translate(list(map(lambda x:x*ratio,trans))) - print(box) - plotter.add_mesh(box, color='yellow',show_edges=True) - return self - def show(self): - plotter.show() - return self - - return foo() - - -if VIS: - vis().add_pc(bg,color='blue').show() - vis().add_pc(bgAndLine,color='blue').show() - -#%%============================================================== -#======================= util functions ========================= -#================================================================ -def crop(pc,ix=None,iy=None,iz=None): - # crop a subrectangle in a pointcloud - # usage: crop(pc, ix = (x_min,x_max), ...) - # return: array(N,3) - mask = True - for dim,intervel in zip([0,1,2],[ix,iy,iz]): - if not intervel: continue - d,u = intervel - mask &= pc[:,dim] >= d - mask &= pc[:,dim] <= u - print(f'points left after cropping {dim}\'th dim',mask.sum()) - return pc[mask] - -from sklearn.linear_model import RANSACRegressor -from sklearn.linear_model import LinearRegression -def fit_plane_ransac(pc,tol=0.01): - # fit a line/plane/hyperplane in a pointcloud - # pc: np array (N,D). the pointcloud - # tol: the tolerance. default 0.01 - model = RANSACRegressor(LinearRegression(), residual_threshold=tol) - model.fit(pc[:,:-1], pc[:,-1]) - a = model.estimator_.coef_ - inter = model.estimator_.intercept_ - class foo: - def plot(self): - inliers = pc[model.inlier_mask_] - if pc.shape[1] == 2: - plt.scatter(pc[:,0], pc[:,1], color='blue', marker='o', s=1) - plt.scatter(inliers[:,0], inliers[:,1], color='red', marker='o', s=1) - x_line = np.linspace(0, 7, 100).reshape(-1,1) - plt.plot(x_line, x_line * a[0] + inter, color='red', label='Fitted Line') - elif pc.shape[1] == 3: - vis().add_pc(pc).add_pc(inliers,color='red').show() - return self - - def results(self): - # return: array(D-1), float, array(N,3) - # ^: , coeffs, intercept toward the plane, inliers of the fit - return a,inter - return foo() - -from scipy.spatial import cKDTree -def pc_diff(pc0,pc1,tol=0.1): - # remove points in pc0 from pc1 s.t. euclidian distance is within tolerance - # return: array(N,3) - tree = cKDTree(pc0) - diff = [] - for i, point in enumerate(pc1): - _, idx = tree.query(point) - distance = np.linalg.norm(point - pc0[idx]) # Compute the distance - if distance > tol: - diff.append(point) - tree = cKDTree(pc1) - for i, point in enumerate(pc0): - _, idx = tree.query(point) - distance = np.linalg.norm(point - pc1[idx]) # Compute the distance - if distance > tol: - diff.append(point) - return np.array(diff) - - - - - - -#%%============================================================== -#========================= tz rx ry ============================= -#================================================================ -# %% this time we crop to keep the ground -cropped_bg = crop(bg,iz = (-3,-2)) -if VIS: - print(cropped_bg.shape) - vis().add_pc(cropped_bg,color='blue').show() - -#%% -from math import sqrt -fit = fit_plane_ransac(cropped_bg,tol=0.01) -c, inter = fit.results() -normv = np.array([c[0], c[1], -1]) -normv /= np.linalg.norm(normv) -nx,ny,nz = normv -height = nz * inter -# TODO MAGIC NUMBER WARNING -height_axel = 0.2794 # 11 inches that we measured -tz = height - height_axel -if VIS: - fit.plot() -ry = math.atan2(nx, sqrt(ny**2 + nz**2)) -rx = math.atan2(-ny,sqrt(nx**2 + nz**2)) - - - -#%%============================================================== -#========================== tx ty rz ============================ -#================================================================ -#%% crop to only keep a frontal box area -area = (-0,7),(-1,1),(-3,1) -cropped_bg = crop(bg,*area) -cropped_bgAndLine = crop(bgAndLine,*area) -print(cropped_bg.shape) -print(cropped_bgAndLine.shape) - -#%% Take difference to only keep added object -diff = pc_diff(cropped_bg,cropped_bgAndLine) -if VIS: - vis().add_pc(diff,color='blue').show() #visualize diff, hopefully the added objects - -# %% use the added objects to find rz. -# TODO after dataset retake -# right now we assume tx = ty = 0 and \ -# just use median to find a headding direction -fit = fit_plane_ransac(diff[:,:2],tol=0.6) -c,inter = fit.results() -if VIS: - fit.plot() -# tx = ty = 0 -# hx,hy = np.median(diff,axis=0)[:2] -# rz = -np.arctan2(hy,hx) -tx = - (2.56 - 1.46) # https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ -ty = - inter -rz = - math.atan(c) - -if VIS: - from scipy.spatial.transform import Rotation as R - rot = R.from_euler('xyz',[0,0,rz]).as_matrix() - - calibrated_bgAndLine = bgAndLine @ rot.T + [tx,ty,0] - line = np.arange(0,100)/100*7 - line = np.stack((line,c[0]*line+inter,np.zeros(100)),axis=1) - line = line @ rot.T + [tx,ty,0] - vis().add_pc(calibrated_bgAndLine,color='blue').add_pc(line,color='red').show() - - -#%% visualize calibrated pointcloud -if VIS: - from scipy.spatial.transform import Rotation as R - rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() - - calibrated_bgAndLine = bgAndLine @ rot.T + [tx,ty,tz] - # projection - # calibrated_bgAndLine[:,2] = 0 - v = vis(ratio=100) - v.add_pc(calibrated_bgAndLine,color='blue') - v.add_box((2.56,.61*2,2.03+height_axel),[2.56/2,0,(2.03+height_axel)/2]) - v.show() -# %% -print(f""" -translation: ({tx,ty,tz}) -rotation: ({rx,ry,rz}) -""") - - -# %% - - -s = np.load('/mount/wp/GEMstack/data/lidar1.npz')['arr_0'] # load null scene -- no added objects -s = s[~np.all(s == 0, axis=1)] -vis().add_pc(s,color='blue').show() -# %% From b21bb65da4825bd2971f3272aa62f636deb9d33e Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Fri, 14 Feb 2025 09:48:23 -0600 Subject: [PATCH 51/71] add logic to avoid unnecessary yielding --- GEMstack/onboard/planning/longitudinal_planning.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 1087bd831..96b4c9514 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -28,8 +28,10 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentStat vehicle_buffer_x = 3.0 vehicle_buffer_y = 1.5 - vehicle_front = curr_x + vehicle_length / 2 - vehicle_back = curr_x - vehicle_length / 2 + yield_buffer_y = 1.0 + + vehicle_front = curr_x + vehicle_length + vehicle_back = curr_x vehicle_left = curr_y + vehicle_width / 2 vehicle_right = curr_y - vehicle_width / 2 @@ -75,6 +77,13 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentStat return False, 0.0 print(relative_v, distance) + + if obj_v_y > 0 and ((obj_y - curr_y) / relative_v) < ((vehicle_right - vehicle_buffer_y - yield_buffer_y - pedestrian_left) / abs(obj_v_y)): + # The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle + return False, 0.0 + if obj_v_y < 0 and ((obj_y - curr_y) / relative_v) < ((pedestrian_right - vehicle_left - vehicle_buffer_y - yield_buffer_y) / abs(obj_v_y)): + # The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle + return False, 0.0 deceleration = relative_v ** 2 / (2 * distance) if deceleration > max_deceleration: From ca087e3c97279cd72868904b576fe92be18de5d9 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Fri, 14 Feb 2025 18:40:12 +0000 Subject: [PATCH 52/71] dumb bug fixed --- GEMstack/offboard/calibration/lidar_to_camera_manual.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/offboard/calibration/lidar_to_camera_manual.py b/GEMstack/offboard/calibration/lidar_to_camera_manual.py index 8d25510dd..4d3cc9719 100644 --- a/GEMstack/offboard/calibration/lidar_to_camera_manual.py +++ b/GEMstack/offboard/calibration/lidar_to_camera_manual.py @@ -84,7 +84,7 @@ def crop(pc,ix=None,iy=None,iz=None): return pc[mask] -lidar_post = np.pad(lidar_points,((0,0),(0,1)),constant_values=1) @ lidar_ex.T +lidar_post = np.pad(lidar_points,((0,0),(0,1)),constant_values=1) @ lidar_ex.T[:,:3] lidar_post = crop(lidar_post,ix=(0,8),iy=(-5,5)) vis().add_pc(lidar_post).show() From ade0559fa1bf80a99b2d1403e6796a6880c7e4e1 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Fri, 14 Feb 2025 00:16:46 -0600 Subject: [PATCH 53/71] Modified braking function for experiment --- .../vehicle/gem_e4_sensor_environment.yaml | 3 +- GEMstack/offboard/calibration/camera_info.py | 41 +++ .../calibration/capture_ouster_oak.py | 102 ++++++++ GEMstack/offboard/calibration/icp.py | 179 +++++++++++++ .../calibration/klampt_lidar_ouster_show.py | 183 +++++++++++++ .../calibration/lidar_to_camera_manual.py | 191 ++++++++++++++ .../calibration/make_gem_e4_ouster_v2.py | 243 ++++++++++++++++++ 7 files changed, 940 insertions(+), 2 deletions(-) create mode 100644 GEMstack/offboard/calibration/camera_info.py create mode 100644 GEMstack/offboard/calibration/capture_ouster_oak.py create mode 100644 GEMstack/offboard/calibration/icp.py create mode 100644 GEMstack/offboard/calibration/klampt_lidar_ouster_show.py create mode 100644 GEMstack/offboard/calibration/lidar_to_camera_manual.py create mode 100644 GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py diff --git a/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml b/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml index a0c611b37..b9e719261 100644 --- a/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml +++ b/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml @@ -2,5 +2,4 @@ ros_topics: top_lidar: /ouster/points front_camera: /oak/rgb/image_raw front_depth: /oak/stereo/image_raw - gnss: /septentrio_gnss/insnavgeod - \ No newline at end of file + gnss: /novatel/inspva diff --git a/GEMstack/offboard/calibration/camera_info.py b/GEMstack/offboard/calibration/camera_info.py new file mode 100644 index 000000000..5610efb7f --- /dev/null +++ b/GEMstack/offboard/calibration/camera_info.py @@ -0,0 +1,41 @@ +# ROS Headers +import rospy +from sensor_msgs.msg import Image,PointCloud2, CameraInfo +import sensor_msgs.point_cloud2 as pc2 +import ctypes +import struct +import pickle +import image_geometry + +import numpy as np +import os +import time + +camera_image = None + +def camera_callback(info : CameraInfo): + global camera_image + camera_image = info + +def get_intrinsics(): + model = image_geometry.PinholeCameraModel() + model.fromCameraInfo(camera_image) + print(model.intrinsicMatrix()) + +def main(folder='data',start_index=1): + rospy.init_node("capture_cam_info",disable_signals=True) + caminfo_sub = rospy.Subscriber("/oak/rgb/camera_info", CameraInfo, camera_callback) + while True: + if camera_image: + time.sleep(1) + get_intrinsics() + +if __name__ == '__main__': + import sys + folder = 'data' + start_index = 1 + if len(sys.argv) >= 2: + folder = sys.argv[1] + if len(sys.argv) >= 3: + start_index = int(sys.argv[2]) + main(folder,start_index) diff --git a/GEMstack/offboard/calibration/capture_ouster_oak.py b/GEMstack/offboard/calibration/capture_ouster_oak.py new file mode 100644 index 000000000..3c9e1e6af --- /dev/null +++ b/GEMstack/offboard/calibration/capture_ouster_oak.py @@ -0,0 +1,102 @@ +# ROS Headers +import rospy +from sensor_msgs.msg import Image,PointCloud2 +import sensor_msgs.point_cloud2 as pc2 +import ctypes +import struct + +# OpenCV and cv2 bridge +import cv2 +from cv_bridge import CvBridge +import numpy as np +import os +import time + +lidar_points = None +camera_image = None +depth = None +depth_image = None +bridge = CvBridge() + +def lidar_callback(lidar : PointCloud2): + global lidar_points + lidar_points = lidar + +def camera_callback(img : Image): + global camera_image + camera_image = img + +def depth_callback(img : Image): + global depth_image + depth_image = img + +def pc2_to_numpy(pc2_msg, want_rgb = False): + gen = pc2.read_points(pc2_msg, skip_nans=True) + if want_rgb: + xyzpack = np.array(list(gen),dtype=np.float32) + if xyzpack.shape[1] != 4: + raise ValueError("PointCloud2 does not have points") + xyzrgb = np.empty((xyzpack.shape[0],6)) + xyzrgb[:,:3] = xyzpack[:,:3] + for i,x in enumerate(xyzpack): + rgb = x[3] + # cast float32 to int so that bitwise operations are possible + s = struct.pack('>f' ,rgb) + i = struct.unpack('>l',s)[0] + # you can get back the float value by the inverse operations + pack = ctypes.c_uint32(i).value + r = (pack & 0x00FF0000)>> 16 + g = (pack & 0x0000FF00)>> 8 + b = (pack & 0x000000FF) + #r,g,b values in the 0-255 range + xyzrgb[i,3:] = (r,g,b) + return xyzrgb + else: + return np.array(list(gen),dtype=np.float32)[:,:3] + +def save_scan(lidar_fn,color_fn,depth_fn): + print("Saving scan to",lidar_fn,color_fn) + + pc = pc2_to_numpy(lidar_points,want_rgb=False) # convert lidar feed to numpy + np.savez(lidar_fn,pc) + cv2.imwrite(color_fn,bridge.imgmsg_to_cv2(camera_image)) + + dimage = bridge.imgmsg_to_cv2(depth_image) + dimage_non_nan = dimage[np.isfinite(dimage)] + print("Depth range",np.min(dimage_non_nan),np.max(dimage_non_nan)) + dimage = np.nan_to_num(dimage,nan=0,posinf=0,neginf=0) + dimage = (dimage/4000*0xffff) + print("Depth pixel range",np.min(dimage),np.max(dimage)) + dimage = dimage.astype(np.uint16) + cv2.imwrite(depth_fn,dimage) + +def main(folder='data',start_index=1): + rospy.init_node("capture_ouster_oak",disable_signals=True) + lidar_sub = rospy.Subscriber("/ouster/points", PointCloud2, lidar_callback) + camera_sub = rospy.Subscriber("/oak/rgb/image_raw", Image, camera_callback) + depth_sub = rospy.Subscriber("/oak/stereo/image_raw", Image, depth_callback) + index = 0 + print(" Storing lidar point clouds as npz") + print(" Storing color images as png") + print(" Storing depth images as tif") + print(" Ctrl+C to quit") + while True: + if camera_image and depth_image: + cv2.imshow("result",bridge.imgmsg_to_cv2(camera_image)) + time.sleep(.5) + files = [ + os.path.join(folder,'lidar{}.npz'.format(index)), + os.path.join(folder,'color{}.png'.format(index)), + os.path.join(folder,'depth{}.tif'.format(index))] + save_scan(*files) + index += 1 + +if __name__ == '__main__': + import sys + folder = 'data' + start_index = 1 + if len(sys.argv) >= 2: + folder = sys.argv[1] + if len(sys.argv) >= 3: + start_index = int(sys.argv[2]) + main(folder,start_index) diff --git a/GEMstack/offboard/calibration/icp.py b/GEMstack/offboard/calibration/icp.py new file mode 100644 index 000000000..376c285a2 --- /dev/null +++ b/GEMstack/offboard/calibration/icp.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python3 + +import os +import glob +import numpy as np +import cv2 +import open3d as o3d + +# --- Adjust these to match your intrinsics and data details --- +INTRINSICS = np.array([ + [684.83331299, 0. , 573.37109375], + [ 0. , 684.60968018, 363.70092773], + [ 0. , 0. , 1. ] +], dtype=np.float32) + +# Depth scale: how to go from raw .tif values to actual "Z" in meters. +# In your capture code, you used: dimage = (dimage/4000*0xffff) +# Possibly each pixel is stored with range 0..65535. Adjust as needed. +DEPTH_SCALE = 4000.0 # Example factor if your depth was in mm or a certain scale. + +def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): + """ + Convert a single-channel depth image into an Nx3 array of 3D points + in the camera coordinate system. + - depth_img: 2D array of type uint16 or float with depth values + - intrinsics: 3x3 camera matrix + """ + fx = intrinsics[0,0] + fy = intrinsics[1,1] + cx = intrinsics[0,2] + cy = intrinsics[1,2] + + # Indices of each pixel + h, w = depth_img.shape + i_range = np.arange(h) + j_range = np.arange(w) + jj, ii = np.meshgrid(j_range, i_range) # shape (h,w) + + # Flatten + ii = ii.flatten().astype(np.float32) + jj = jj.flatten().astype(np.float32) + depth = depth_img.flatten().astype(np.float32) + + # Filter out zero / invalid depths + valid_mask = (depth > 0) + ii = ii[valid_mask] + jj = jj[valid_mask] + depth = depth[valid_mask] + + # Reproject to 3D (camera frame) + # X = (x - cx) * Z / fx, Y = (y - cy) * Z / fy, Z = depth + # Note: Z must be in meters or consistent units + z = depth / DEPTH_SCALE # Convert from your .tif scale to real meters + x = (jj - cx) * z / fx + y = (ii - cy) * z / fy + points3d = np.stack((x, y, z), axis=-1) # shape (N,3) + + return points3d + +def load_lidar_points(npz_file: str): + """ + Load the Nx3 LiDAR points from a .npz file created by 'np.savez'. + """ + data = np.load(npz_file) + # The capture code used: np.savez(lidar_fn, pc) + # So 'data' might have the default key 'arr_0' or 'pc' if named + # Inspect data.files to see. Let's assume 'arr_0' or single key: + arr_key = data.files[0] + points = data[arr_key] + # shape check, must be Nx3 or Nx4, ... + return points + +def make_open3d_pcd(points: np.ndarray): + """ + Convert Nx3 numpy array into an Open3D point cloud object. + """ + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + return pcd + +def perform_icp(camera_pcd: o3d.geometry.PointCloud, + lidar_pcd: o3d.geometry.PointCloud): + """ + Perform local ICP alignment of camera_pcd (source) to lidar_pcd (target). + Returns a transformation 4x4 that maps camera -> lidar (or vice versa). + """ + # 1) Estimate normals if you want point-to-plane + lidar_pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=1.0, max_nn=30)) + camera_pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=1.0, max_nn=30)) + + # 2) Initial guess: identity or something better if you have one + init_guess = np.eye(4) + + # 3) Choose a threshold for inlier distance + threshold = 0.5 # adjust based on your environment scale + + # 4) Run ICP (point-to-plane or point-to-point) + print("[ICP] Running ICP alignment ...") + result = o3d.pipelines.registration.registration_icp( + camera_pcd, # source + lidar_pcd, # target + threshold, + init_guess, + estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane() + ) + + print("[ICP] Done. Fitness = %.4f, RMSE = %.4f" % (result.fitness, result.inlier_rmse)) + print("[ICP] Transformation:\n", result.transformation) + return result.transformation + +def main(folder='data'): + color_files = sorted(glob.glob(os.path.join(folder, "color*.png"))) + icp_results = [] # to store (index, transform, fitness, rmse) + + for color_path in color_files: + # Extract index from filename, e.g. color10.png -> 10 + basename = os.path.basename(color_path) + idx_str = basename.replace("color","").replace(".png","") + + if int(idx_str) in range(77): + depth_path = os.path.join(folder, f"depth{idx_str}.tif") + lidar_path = os.path.join(folder, f"lidar{idx_str}.npz") + + if not (os.path.exists(depth_path) and os.path.exists(lidar_path)): + continue + + # Load depth / convert to 3D + depth_img = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED) + if depth_img is None: + continue + camera_points = depth_to_points(depth_img, INTRINSICS) + camera_pcd = make_open3d_pcd(camera_points) + + # Load LiDAR + lidar_points = load_lidar_points(lidar_path) + lidar_pcd = make_open3d_pcd(lidar_points) + + lidar_pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=1.0, # Adjust based on your scene scale + max_nn=30 + ) + ) + + # Also estimate normals on the Camera (source) cloud (recommended) + camera_pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=1.0, + max_nn=30 + ) + ) + + # Now you can run Point-to-Plane ICP + init_guess = np.eye(4) + threshold = 0.5 # or some appropriate distance + result_icp = o3d.pipelines.registration.registration_icp( + camera_pcd, + lidar_pcd, + threshold, + init_guess, + o3d.pipelines.registration.TransformationEstimationPointToPlane() + ) + + print("ICP result:", result_icp.transformation) + print("Fitness:", result_icp.fitness, " RMSE:", result_icp.inlier_rmse) + + # After processing all frames, we can analyze or average + if len(icp_results) > 0: + # Example: pick the best frame by highest fitness + best_frame = max(icp_results, key=lambda x: x[2]) # x[2] is fitness + print("\nBest frame by fitness was index=", best_frame[0], + " with fitness=", best_frame[2], " rmse=", best_frame[3]) + else: + print("No frames processed properly.") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py b/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py new file mode 100644 index 000000000..11a9fcbcc --- /dev/null +++ b/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py @@ -0,0 +1,183 @@ +from klampt import vis +from klampt.math import so3,se3 +from klampt.vis.colorize import colorize +from klampt import PointCloud,Geometry3D +from klampt.io import numpy_convert +from klampt.model.sensing import image_to_points +from klampt.model.create import bbox +import klampt +import cv2 +import os +import numpy as np +import math +import time + +#uncalibrated values -- TODO: load these from a calibration file +zed_K = np.array([[684.8333, 0.0, 573.371], + [0.0, 684.609, 363.7], + [0.0, 0.0, 1.0]]) +zed_intrinsics = [zed_K[0,0],zed_K[1,1],zed_K[0,2],zed_K[1,2]] +zed_w = 1152 +zed_h = 720 + + +def main(folder): + lidar_xform = se3.identity() + zed_xform = (so3.from_ndarray(np.array([[0,0,1],[-1,0,0],[0,-1,0]])),[0,0,0]) + lidar_pattern = os.path.join(folder,"lidar{}.npz") + color_pattern = os.path.join(folder,"color{}.png") + depth_pattern = os.path.join(folder,"depth{}.tif") + data = {} + def load_and_show_scan(idx): + arr_compressed = np.load(lidar_pattern.format(idx)) + arr = arr_compressed['arr_0'] + arr_compressed.close() + pc = numpy_convert.from_numpy(arr,'PointCloud') + pc = colorize(pc,'z','plasma') + data['lidar'] = Geometry3D(pc) + + try: # might need some modifications to work with our code + color = cv2.imread(color_pattern.format(idx)) + depth = cv2.imread(depth_pattern.format(idx),cv2.IMREAD_UNCHANGED) + depth = depth.astype(np.float32) + print("depth range",np.min(depth),np.max(depth)) + zed_xfov = 2*np.arctan(zed_w/(2*zed_intrinsics[0])) + zed_yfov = 2*np.arctan(zed_h/(2*zed_intrinsics[1])) + print("estimated zed horizontal FOV",math.degrees(zed_xfov),"deg") + print(f"Depth image shape: {depth.shape}, dtype: {depth.dtype}, min: {np.min(depth)}, max: {np.max(depth)}") + print(f"Color image shape: {color.shape}, dtype: {color.dtype}, min: {np.min(color)}, max: {np.max(color)}") + image_to_points( + depth, + color, + intrinsics=None, + xfov=zed_xfov, + yfov=zed_yfov, + depth_scale=4000.0 / 0xffff + ) + + except Exception as e: + print("Error loading zed data:",e) + pc = PointCloud() + + data['oak'] = Geometry3D(pc) + data['lidar'].setCurrentTransform(*lidar_xform) + data['oak'].setCurrentTransform(*zed_xform) + vis.add('lidar',data['lidar']) + vis.add('oak',data['oak']) + + data['index'] = 1 + def increment_index(): + data['index'] += 1 + try: + load_and_show_scan(data['index']) + except Exception: + data['index'] -= 1 + return + def decrement_index(): + data['index'] -= 1 + try: + load_and_show_scan(data['index']) + except Exception: + data['index'] += 1 + return + def print_xforms(): + print("lidar:") + print("rotation:",so3.ndarray(lidar_xform[0])) + print("position:",lidar_xform[1]) + print("zed:") + print("rotation:",so3.ndarray(zed_xform[0])) + print("position:",zed_xform[1]) + + point_selection = (4,0,0) + box_selection = [(3.5,-0.5,-0.5),(4.5,0.5,0.5)] + box_geometry = bbox(box_selection[0],box_selection[1],type='GeometricPrimitive') + data['selection_widget'] = None + + def select_point(): + if data['selection_widget'] is not None: + vis.remove(data['selection_widget']) + data['selection_widget'] = 'point_widget' + vis.add('point_widget',point_selection) + vis.edit('point_widget') + + def select_box(): + if data['selection_widget'] is not None: + vis.remove(data['selection_widget']) + data['selection_widget'] = 'box_widget' + vis.add('box_widget',box_geometry,color=(1.0,0.5,0,0.5)) + vis.add('box_widget_handle1',box_selection[0]) + vis.edit('box_widget_handle1') + vis.add('box_widget_handle2',box_selection[1]) + vis.edit('box_widget_handle2') + + def print_selection(): + if data['selection_widget'] is not None: + if data['selection_widget'] == 'point_widget': + print("Target point:",point_selection) + lidar = data['lidar'] # type: Geometry3D + pts = lidar.getPointCloud().getPoints() + pts = pts @ so3.ndarray(lidar_xform[0]).T + np.array(lidar_xform[1]) + dist_2 = np.sum((pts - point_selection)**2, axis=1) + closest_dist2 = np.min(dist_2) + closest_ind = np.argmin(dist_2) + print("Closest lidar point is",pts[closest_ind],"index",closest_ind,"at distance",math.sqrt(closest_dist2)) + else: + print("Target box:",box_selection) + lidar = data['lidar'] # type: Geometry3D + pts = lidar.getPointCloud().getPoints() + pts = pts @ so3.ndarray(lidar_xform[0]).T + np.array(lidar_xform[1]) + mask = np.logical_and(np.all(pts >= box_selection[0],axis=1),np.all(pts <= box_selection[1],axis=1)) + print("Number of lidar points in box:",np.sum(mask)) + print("Indices of lidar points in box:",np.where(mask)[0]) + print("Points in box",pts[mask]) + else: + print("No selection") + + vis.addAction(select_point,'Select with point','s','Select a point by dragging a point widget') + vis.addAction(select_box,'Select with box','b','Select multiple points with a box widget') + vis.addAction(print_selection,'Print selection','q') + + vis.addAction(increment_index,"Increment index",'=') + vis.addAction(decrement_index,"Decrement index",'-') + vis.addAction(print_xforms,'Print transforms','p') + load_and_show_scan(1) + vis.add('zed_xform',zed_xform) + vis.add('lidar_xform',lidar_xform) + vis.edit('zed_xform') + vis.edit('lidar_xform') + vis.show() + while vis.shown(): + lidar_xform = vis.getItemConfig('lidar_xform') + lidar_xform = lidar_xform[:9],lidar_xform[9:] + zed_xform = vis.getItemConfig('zed_xform') + zed_xform = zed_xform[:9],zed_xform[9:] + if data['selection_widget'] == 'point_widget': + point_selection = vis.getItemConfig('point_widget') + elif data['selection_widget'] == 'box_widget': + handle1 = vis.getItemConfig('box_widget_handle1') + handle2 = vis.getItemConfig('box_widget_handle2') + lower = [min(handle1[i],handle2[i]) for i in range(3)] + upper = [max(handle1[i],handle2[i]) for i in range(3)] + box_selection = [lower,upper] + box_geometry = bbox(lower,upper,type='GeometricPrimitive') + vis.add('box_widget',box_geometry,color=(1.0,0.5,0,0.5)) + data['lidar'].setCurrentTransform(*lidar_xform) + data['oak'].setCurrentTransform(*zed_xform) + time.sleep(0.02) + vis.kill() + +if __name__ == '__main__': + import sys + folder = 'data' + if len(sys.argv) >= 2: + folder = sys.argv[1] + if len(sys.argv) >= 3: + calib = sys.argv[2] + import yaml + with open(calib,'r') as f: + config = yaml.load(f,yaml.SafeLoader) + zed_K = np.array(config['K']).reshape((3,3)) + zed_intrinsics = [zed_K[0,0],zed_K[1,1],zed_K[0,2],zed_K[1,2]] + zed_w = config['width'] + zed_height = config['height'] + main(folder) diff --git a/GEMstack/offboard/calibration/lidar_to_camera_manual.py b/GEMstack/offboard/calibration/lidar_to_camera_manual.py new file mode 100644 index 000000000..4d3cc9719 --- /dev/null +++ b/GEMstack/offboard/calibration/lidar_to_camera_manual.py @@ -0,0 +1,191 @@ +#%% +import cv2 +from scipy.spatial.transform import Rotation as R +import matplotlib.pyplot as plt +import numpy as np + +rgb_path = '/mount/wp/GEMstack/data/color21.png' +depth_path = '/mount/wp/GEMstack/data/depth21.tif' +lidar_path = '/mount/wp/GEMstack/data/lidar21.npz' + +img = cv2.imread(rgb_path, cv2.IMREAD_UNCHANGED) + +lidar_points = np.load(lidar_path)['arr_0'] +lidar_points = lidar_points[~np.all(lidar_points== 0, axis=1)] # remove (0,0,0)'s + +rx,ry,rz = 0.006898647163954201, 0.023800082245145304, -0.025318355743942974 +tx,ty,tz = -1.1, 0.037735827433173136, 1.953202227766785 +rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() +lidar_ex = np.hstack([rot,[[tx],[ty],[tz]]]) +lidar_ex = np.vstack([lidar_ex,[0,0,0,1]]) + +camera_in = np.array([ + [684.83331299, 0. , 573.37109375], + [ 0. , 684.60968018, 363.70092773], + [ 0. , 0. , 1. ] +], dtype=np.float32) + +#%% +# blurred = cv2.GaussianBlur(img, (5, 5), 0) +# edges = cv2.Canny(blurred, threshold1=50, threshold2=150) +# plt.imshow(edges) +plt.imshow(cv2.cvtColor(img,cv2.COLOR_BGR2RGB)) + +#%% +import pyvista as pv +def vis(title='', ratio=1): + print(title) + pv.set_jupyter_backend('client') + plotter = pv.Plotter(notebook=True) + plotter.show_axes() + class foo: + def set_cam(self,pos=(-20*ratio,0,20*ratio),foc=(0,0,0)): + plotter.camera.position = pos + plotter.camera.focal_point = foc + return self + def add_pc(self,pc,ratio=ratio,**kargs): + plotter.add_mesh( + pv.PolyData(pc*ratio), + render_points_as_spheres=True, + point_size=2, + **kargs) + return self + def add_line(self,p1,p2,ratio=ratio,**kargs): + plotter.add_mesh( + pv.Line(p1*ratio,p2*ratio), + **kargs, + line_width=1) + return self + def add_box(self,bound,trans,ratio=ratio): + l,w,h = map(lambda x:x*ratio,bound) + box = pv.Box(bounds=(-l/2,l/2,-w/2,w/2,-h/2,h/2)) + box = box.translate(list(map(lambda x:x*ratio,trans))) + plotter.add_mesh(box, color='yellow') + return self + def show(self): + plotter.show() + return self + def close(self): + plotter.close() + return None + + return foo().set_cam() +def crop(pc,ix=None,iy=None,iz=None): + # crop a subrectangle in a pointcloud + # usage: crop(pc, ix = (x_min,x_max), ...) + # return: array(N,3) + mask = True + for dim,intervel in zip([0,1,2],[ix,iy,iz]): + if not intervel: continue + d,u = intervel + mask &= pc[:,dim] >= d + mask &= pc[:,dim] <= u + print(f'points left after cropping {dim}\'th dim',mask.sum()) + return pc[mask] + + +lidar_post = np.pad(lidar_points,((0,0),(0,1)),constant_values=1) @ lidar_ex.T[:,:3] +lidar_post = crop(lidar_post,ix=(0,8),iy=(-5,5)) +vis().add_pc(lidar_post).show() + +#%% +def pick_4_img(img): + corners = [] # Reset the corners list + def click_event(event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN: + corners.append((x, y)) + cv2.circle(param, (x, y), 5, (0, 255, 0), -1) + cv2.imshow('Image', param) + + cv2.imshow('Image', img) + cv2.setMouseCallback('Image', click_event, img) + + while True: + if len(corners) == 4: + break + if cv2.waitKey(1) & 0xFF == ord('q'): + return None + + cv2.destroyAllWindows() + + return corners +cpoints = np.array(pick_4_img(img)).astype(float) +print(cpoints) + +#%% +def pick_4_pc(point_cloud): + points = [] + def cb(pt,*args): + points.append(pt) + while len(points)!=4: + points = [] + cloud = pv.PolyData(point_cloud) + plotter = pv.Plotter(notebook=False) + plotter.camera.position = (-20,0,20) + plotter.camera.focal_point = (0,0,0) + plotter.add_mesh(cloud, color='lightblue', point_size=5, render_points_as_spheres=True) + plotter.enable_point_picking(cb) + plotter.show() + return points + +lpoints = np.array(pick_4_pc(lidar_post)) +print(lpoints) +# %% +success, rvec, tvec = cv2.solvePnP(lpoints, cpoints, camera_in, None) +R, _ = cv2.Rodrigues(rvec) + +T = np.eye(4) +T[:3, :3] = R +T[:3, 3] = tvec.flatten() +print(T) + + +#%% +def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): + """ + Convert a single-channel depth image into an Nx3 array of 3D points + in the camera coordinate system. + - depth_img: 2D array of type uint16 or float with depth values + - intrinsics: 3x3 camera matrix + """ + fx = intrinsics[0,0] + fy = intrinsics[1,1] + cx = intrinsics[0,2] + cy = intrinsics[1,2] + + # Indices of each pixel + h, w = depth_img.shape + i_range = np.arange(h) + j_range = np.arange(w) + jj, ii = np.meshgrid(j_range, i_range) # shape (h,w) + + # Flatten + ii = ii.flatten().astype(np.float32) + jj = jj.flatten().astype(np.float32) + depth = depth_img.flatten().astype(np.float32) + + # Filter out zero / invalid depths + valid_mask = (depth > 0) + ii = ii[valid_mask] + jj = jj[valid_mask] + depth = depth[valid_mask] + + z = depth / 10000 + x = (jj - cx) * z / fx + y = (ii - cy) * z / fy + points3d = np.stack((x, y, z), axis=-1) # shape (N,3) + + return points3d + + + +depth_img = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED) +camera_points = depth_to_points(depth_img, camera_in) + +v=vis() +v.add_pc(np.pad(lidar_points,((0,0),(0,1)),constant_values=1)@lidar_ex.T@T.T[:,:3],color='blue') +v.add_pc(camera_points,color='red') +v.show() + +#%% +print(np.vstack([(lidar_ex.T@T.T[:,:3]).T,[0,0,0,1]])) \ No newline at end of file diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py b/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py new file mode 100644 index 000000000..6e0b25940 --- /dev/null +++ b/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py @@ -0,0 +1,243 @@ +#%% +import os +import sys +import math +import numpy as np +from scipy.spatial.transform import Rotation as R +os.getcwd() +VIS = False # True to show visuals + +#%% things to extract +tx,ty,tz,rx,ry,rz = [None] * 6 + +#%%============================================================== +#======================= util functions ========================= +#================================================================ +if VIS: + import pyvista as pv + import matplotlib.pyplot as plt +def vis(title='', ratio=1): + print(title) + pv.set_jupyter_backend('client') + plotter = pv.Plotter(notebook=True) + plotter.camera.position = (-20*ratio,0,20*ratio) + plotter.camera.focal_point = (0,0,0) + plotter.show_axes() + class foo: + def add_pc(self,pc,ratio=ratio,**kargs): + plotter.add_mesh( + pv.PolyData(pc*ratio), + render_points_as_spheres=True, + point_size=2, + **kargs) + return self + def add_line(self,p1,p2,ratio=ratio,**kargs): + plotter.add_mesh( + pv.Line(p1*ratio,p2*ratio), + **kargs, + line_width=1) + return self + def add_box(self,bound,trans,ratio=ratio): + l,w,h = map(lambda x:x*ratio,bound) + box = pv.Box(bounds=(-l/2,l/2,-w/2,w/2,-h/2,h/2)) + box = box.translate(list(map(lambda x:x*ratio,trans))) + plotter.add_mesh(box, color='yellow') + return self + def show(self): + plotter.show() + return self + + return foo() +def load_scene(path): + sc = np.load(path)['arr_0'] + sc = sc[~np.all(sc == 0, axis=1)] # remove (0,0,0)'s + return sc +def crop(pc,ix=None,iy=None,iz=None): + # crop a subrectangle in a pointcloud + # usage: crop(pc, ix = (x_min,x_max), ...) + # return: array(N,3) + mask = True + for dim,intervel in zip([0,1,2],[ix,iy,iz]): + if not intervel: continue + d,u = intervel + mask &= pc[:,dim] >= d + mask &= pc[:,dim] <= u + print(f'points left after cropping {dim}\'th dim',mask.sum()) + return pc[mask] + +from sklearn.linear_model import RANSACRegressor +from sklearn.linear_model import LinearRegression +def fit_plane_ransac(pc,tol=0.01): + # fit a line/plane/hyperplane in a pointcloud + # pc: np array (N,D). the pointcloud + # tol: the tolerance. default 0.01 + model = RANSACRegressor(LinearRegression(), residual_threshold=tol) + model.fit(pc[:,:-1], pc[:,-1]) + a = model.estimator_.coef_ + inter = model.estimator_.intercept_ + class foo: + def plot(self): + inliers = pc[model.inlier_mask_] + if pc.shape[1] == 2: + plt.title('ransac fitting line') + plt.scatter(pc[:,0], pc[:,1], color='blue', marker='o', s=1) + plt.scatter(inliers[:,0], inliers[:,1], color='red', marker='o', s=1) + x_line = np.linspace(0, max(pc[:,0]), 100).reshape(-1,1) + plt.plot(x_line, x_line * a[0] + inter, color='red', label='Fitted Line') + elif pc.shape[1] == 3: + vis('ransac fitting a plane').add_pc(pc).add_pc(inliers,color='red').show() + return self + + def results(self): + # return: array(D-1), float, array(N,3) + # ^: , coeffs, intercept toward the plane, inliers of the fit + return a,inter + return foo() + +from scipy.spatial import cKDTree +def pc_diff(pc0,pc1,tol=0.1): + # remove points in pc0 from pc1 s.t. euclidian distance is within tolerance + # return: array(N,3) + tree = cKDTree(pc0) + diff = [] + for i, point in enumerate(pc1): + _, idx = tree.query(point) + distance = np.linalg.norm(point - pc0[idx]) # Compute the distance + if distance > tol: + diff.append(point) + tree = cKDTree(pc1) + for i, point in enumerate(pc0): + _, idx = tree.query(point) + distance = np.linalg.norm(point - pc1[idx]) # Compute the distance + if distance > tol: + diff.append(point) + return np.array(diff) + + + + + + +#%%============================================================== +#========================= tz rx ry ============================= +#================================================================ + +#%% load scene for ground plane +sc = load_scene('/mount/wp/GEMstack/data/lidar70.npz') + +# %% we crop to keep the ground +cropped_sc = crop(sc,iz = (-3,-2)) +if VIS: + vis('ground points cropped').add_pc(cropped_sc,color='blue').show() + +#%% +from math import sqrt +fit = fit_plane_ransac(cropped_sc,tol=0.01) # small tol because the ground is very flat +c, inter = fit.results() +normv = np.array([c[0], c[1], -1]) +normv /= np.linalg.norm(normv) +nx,ny,nz = normv +height = nz * inter +# TODO MAGIC NUMBER WARNING +height_axel = 0.2794 # 11 inches that we measured +tz = height - height_axel +if VIS: + fit.plot() +rx = -math.atan2(ny,-nz) +ry = -math.atan2(-nx,-nz) + + +if VIS: + from scipy.spatial.transform import Rotation as R + rot = R.from_euler('xyz',[rx,ry,0]).as_matrix() + cal_sc = sc @ rot.T + [0,0,tz] + vis('yz projection').add_pc(cal_sc*[0,1,1],color='blue').show() + vis('xz projection').add_pc(cal_sc*[1,0,1],color='blue').show() + +#%%============================================================== +#========================== tx ty rz ============================ +#================================================================ + +rot = R.from_euler('xyz',[rx,ry,0]).as_matrix() + +if False: # True to use the diff method to extract object. + # load data + sc0 = load_scene('/mount/wp/GEMstack/data/lidar70.npz') + sc1 = load_scene('/mount/wp/GEMstack/data/lidar78.npz') + + sc0 = sc0 @ rot.T + [0,0,tz] + sc1 = sc1 @ rot.T + [0,0,tz] + + # crop to only keep a frontal box area + area = (-0,7),(-1,1),(-3,1) + cropped0 = crop(sc0,*area) + cropped1 = crop(sc1,*area) + print(cropped0.shape) + print(cropped1.shape) + + # Take difference to only keep added object + objects = pc_diff(cropped0,cropped1) + +else: #False to use only cropping + sc1 = load_scene('/mount/wp/GEMstack/data/lidar1.npz') + + objects = sc1 @ rot.T + [0,0,tz] + + # crop to only keep a frontal box area + area = (-0,20),(-1,1),(0,1) + objects = crop(objects,*area) + print(objects.shape) + + +#%% +if VIS: + vis().add_pc(objects,color='blue').show() #visualize diff, hopefully the added objects + +# %% use the added objects to find rz. +# TODO after dataset retake +# right now we assume tx = ty = 0 and \ +# just use median to find a headding direction +fit = fit_plane_ransac(objects[:,:2],tol=1) # tol=1 because 1m^3 foam boxes +c,inter = fit.results() +if VIS: + fit.plot() +# tx = ty = 0 +# hx,hy = np.median(diff,axis=0)[:2] +# rz = -np.arctan2(hy,hx) +tx = - (2.56 - 1.46) # https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ +ty = - inter +rz = - math.atan(c) + +if VIS: + p1 = (0,inter,0) + p2 = max(objects[:,0])*np.array([1,c[0],0])+np.array([0,inter,0]) + vis().add_pc(sc1*np.array([1,1,0]),color='blue').add_line(p1,p2,color='red').show() + + from scipy.spatial.transform import Rotation as R + rot = R.from_euler('xyz',[0,0,rz]).as_matrix() + cal_sc1 = sc1 @ rot.T + [tx,ty,0] + vis().add_pc(cal_sc1,color='blue').show() + + +#%% visualize calibrated pointcloud +if VIS: + rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() + + cal_sc1 = sc1 @ rot.T + [tx,ty,tz] + # projection + # cal_sc1[:,1] = 0 + v = vis(ratio=100) + v.add_pc(cal_sc1*[0,1,1],color='blue') + v.add_box((2.56,.61*2,2.03+height_axel),[2.56/2,0,(2.03+height_axel)/2]) + v.show() + # the yellow box should be 11 inches above the ground + # rear-axel center should be at (0,0,0) +# %% +print(f""" +translation: ({tx,ty,tz}) +rotation: ({rx,ry,rz}) +""") + + + +# %% From 46b6b04ababd44147e7ed43401d00985ab6af579 Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Fri, 14 Feb 2025 16:55:48 -0600 Subject: [PATCH 54/71] modify code for integrating with other algorithm --- .../onboard/planning/longitudinal_planning.py | 39 ++++++++++++++++--- 1 file changed, 33 insertions(+), 6 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 96b4c9514..f851dea97 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -80,18 +80,45 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentStat if obj_v_y > 0 and ((obj_y - curr_y) / relative_v) < ((vehicle_right - vehicle_buffer_y - yield_buffer_y - pedestrian_left) / abs(obj_v_y)): # The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle + print("The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") return False, 0.0 if obj_v_y < 0 and ((obj_y - curr_y) / relative_v) < ((pedestrian_right - vehicle_left - vehicle_buffer_y - yield_buffer_y) / abs(obj_v_y)): # The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle + print("The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") return False, 0.0 - deceleration = relative_v ** 2 / (2 * distance) - if deceleration > max_deceleration: - return True, max_deceleration - if deceleration < min_deceleration: - return False, 0.0 + if obj_v_y != 0: + if obj_v_y < 0: + # The object is moving toward the right side of the vehicle + distance_to_pass = obj_y - (vehicle_right - vehicle_buffer_y - yield_buffer_y) + pedestrian_width / 2 + elif obj_v_y > 0: + # The object is moving toward the left side of the vehicle + distance_to_pass = (vehicle_left + vehicle_buffer_y + yield_buffer_y) - obj_y + pedestrian_width / 2 + + time_to_pass = distance_to_pass / abs(obj_v_y) + + distance_to_move = pedestrian_back - vehicle_front - vehicle_buffer_x + time_to_pass * obj_v_y + + #======replace this part with actual algorithm==== + + deceleration = relative_v ** 2 / (2 * distance) + if deceleration > max_deceleration: + return True, max_deceleration + if deceleration < min_deceleration: + return False, 0.0 + + return True, deceleration + + # ================================================ + + else: + deceleration = relative_v ** 2 / (2 * distance) + if deceleration > max_deceleration: + return True, max_deceleration + if deceleration < min_deceleration: + return False, 0.0 - return True, deceleration + return True, deceleration 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 From 2b6c1d8fa9cd7ee748f809086ed19b773064fd9a Mon Sep 17 00:00:00 2001 From: Simon Date: Fri, 14 Feb 2025 17:12:18 -0600 Subject: [PATCH 55/71] longitudinal plan now taking into account triangle velocity profile cases. Fixed a typo in the plotting of velocities as a debug --- .../onboard/planning/longitudinal_planning.py | 96 +++++++++++++++---- testing/test_longitudinal_plan.py | 3 +- 2 files changed, 79 insertions(+), 20 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index fad1f2875..cbd26a988 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -51,15 +51,17 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m new_times.append(cur_time) velocities.append(current_speed) print("=====================================") - print("new points: ", new_points) - print("current index: ", cur_index) - print("current speed: ", current_speed) + # print("new points: ", new_points) + # print("current index: ", cur_index) + # print("current speed: ", current_speed) + # print("current position: ", cur_point) # Information we will need: # Calculate how much time it would take to stop # Calculate how much distance it would take to stop min_delta_t_stop = current_speed/deceleration min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 + # print(min_delta_x_stop) assert min_delta_x_stop >= 0 @@ -79,7 +81,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m # keep breaking until the next milestone in path if next_point[0] <= points[-1][0]: - print("continuing to next point") + # print("continuing to next point") delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) cur_time += delta_t_to_next_x cur_point = next_point @@ -100,7 +102,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m # because the first if-statement covers for when we decelerating, # the only time current_speed < max_speed is when we are accelerating elif current_speed < max_speed: - print("In case two") + # print("In case two") # next point next_point = points[cur_index+1] # accelerate to max speed @@ -110,21 +112,44 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m # calculate the distance it would take to reach max speed delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 + delta_t_to_stop_from_max_speed = max_speed/deceleration + delta_x_to_stop_from_max_speed = max_speed*delta_t_to_stop_from_max_speed - 0.5*deceleration*delta_t_to_stop_from_max_speed**2 + + delta_t_to_next_point = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + velocity_at_next_point = current_speed + delta_t_to_next_point*acceleration + time_to_stop_from_next_point = velocity_at_next_point/deceleration + delta_x_to_stop_from_next_point = velocity_at_next_point*time_to_stop_from_next_point - 0.5*deceleration*time_to_stop_from_next_point**2 # if we would reach max speed after the next point, # just move to the next point and update the current speed and time - if cur_point[0] + delta_x_to_max_speed >= next_point[0]: - print("go to next point") + if next_point[0] + delta_x_to_stop_from_next_point < points[-1][0] and \ + cur_point[0] + delta_x_to_max_speed >= next_point[0]: + # ("go to next point") # accelerate to max speed delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) cur_time += delta_t_to_next_x cur_point = [next_point[0], 0] current_speed += delta_t_to_next_x*acceleration cur_index += 1 + + # This is the case where we would need to start breaking before reaching + # top speed and before the next point (i.e. triangle shape velocity) + elif cur_point[0] + delta_x_to_max_speed + delta_x_to_stop_from_max_speed >= points[-1][0]: + # print(delta_x_to_max_speed) + # print(delta_x_to_stop_from_max_speed) + # Add a new point at the point where we should start breaking + # print("Adding new point to start breaking") + delta_t_to_next_x = compute_time_triangle(cur_point[0], points[-1][0], current_speed, 0, acceleration, deceleration) + # print(delta_t_to_next_x) + #delta_t_to_next_x = compute_time_to_x(cur_point[0], points[-1][0] - min_delta_x_stop, current_speed, acceleration) + next_x = cur_point[0] + current_speed*delta_t_to_next_x + 0.5*acceleration*delta_t_to_next_x**2 + cur_time += delta_t_to_next_x + cur_point = [next_x, 0] + current_speed += delta_t_to_next_x*acceleration # this is the case where we would reach max speed before the next point # we need to create a new point where we would reach max speed else: - print("adding new point") + # print("adding new point") # we would need to add a new point at max speed cur_time += delta_t_to_max_speed cur_point = [cur_point[0] + delta_x_to_max_speed, 0] @@ -137,17 +162,17 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m elif current_speed == max_speed: next_point = points[cur_index+1] # continue on with max speed - print("In case three") + # print("In case three") # add point to start decelerating if next_point[0] + min_delta_x_stop >= points[-1][0]: - print("Adding new point to start decelerating") + # print("Adding new point to start decelerating") cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed cur_point = [points[-1][0] - min_delta_x_stop,0] current_speed = max_speed else: # Continue on to next point - print("Continuing on to next point") + # print("Continuing on to next point") cur_time += (next_point[0] - cur_point[0])/current_speed cur_point = next_point cur_index += 1 @@ -158,7 +183,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m # We need to hit the breaks next_point = points[cur_index+1] - print("In case four") + # print("In case four") # slow down to max speed delta_t_to_max_speed = (current_speed - max_speed)/deceleration delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 @@ -209,16 +234,16 @@ def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: # 0.5*a*t^2 + v0*t + x0 - x1 = 0 # t = (-v0 + sqrt(v0^2 - 4*0.5*a*(x0-x1)))/(2*0.5*a) # t = (-v0 + sqrt(v0^2 + 2*a*(x1-x0)))/a - print("x0: ", x0) - print("x1: ", x1) - print("v: ", v) - print("a: ", a) + # print("x0: ", x0) + # print("x1: ", x1) + # print("v: ", v) + # print("a: ", a) t1 = (-v + max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a t2 = (-v - max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a - print("t1: ", t1) - print("t2: ", t2) + # print("t1: ", t1) + # print("t2: ", t2) if math.isnan(t1): t1 = 0 if math.isnan(t2): t2 = 0 @@ -232,6 +257,41 @@ def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: else: return 0.0 +def compute_time_triangle(x0 : float, xf: float, v0: float, vf : float, acceleration : float, deceleration : float) -> float: + """ + Compute the time to go from current point assuming we are accelerating to the point at which + we would need to start breaking in order to reach the final point with velocity 0.""" + # xf = v0*t1 + .5a*t1^2 + v1t2 -0.5d*t2^2 + # 0 = v1 - d*t2 = v0 + a*t1 - d * t2 + # t1 = (d*t2 - v0)/a + # xf = v0*(d*t2 - v0)/a + 0.5*a*(d*t2 - v0)^2/a^2 + v1*t2 - 0.5*d*t2^2 + # xf = v0*d*t2/a - v0^2/a + 0.5*a*(d*t2^2 - 2*v0*d*t2 + v0^2)/a^2 + v1*t2 - 0.5*d*t2^2 + # 0 = t2^2(0.5*a*d/a^2 - 0.5*d) + t2(v0*d/a - v0*d*a/a^2 + v1) -v0^2/a +0.5*a*v0^2/a^2 -xf + roots = quad_root(0.5*acceleration + acceleration**2/deceleration - 0.5*acceleration**2/deceleration, + v0+2*acceleration*v0/deceleration - acceleration*v0/deceleration, + x0 - xf + v0**2/deceleration - 0.5*v0**2/deceleration) + print(roots) + t1 = max(roots) + assert t1 > 0 + return t1 + +def quad_root(a : float, b : float, c : float) -> float: + x1 = (-b + max(0,(b**2 - 4*a*c))**0.5)/(2*a) + x2 = (-b - max(0,(b**2 - 4*a*c))**0.5)/(2*a) + + # print("t1: ", t1) + # print("t2: ", t2) + + if math.isnan(x1): x1 = 0 + if math.isnan(x2): x2 = 0 + + # print("t1: ", t1) + # print("t2: ", t2) + + valid_roots = [n for n in [x1, x2] if not type(n) is complex] + # print(f"Valid roots {valid_roots}") + return valid_roots + 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() diff --git a/testing/test_longitudinal_plan.py b/testing/test_longitudinal_plan.py index 481da2226..ab16ca5e6 100644 --- a/testing/test_longitudinal_plan.py +++ b/testing/test_longitudinal_plan.py @@ -34,13 +34,12 @@ def test_longitudinal_planning(): plt.show() plt.plot(test_traj.times,[v for v in test_traj.velocities]) - 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('velocity') plt.show() - test_traj = longitudinal_plan(test_path, 1.0, 2.0, 3.0, 0.0) + test_traj = longitudinal_plan(test_path, 1.1, 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") From 20b73b118f84abc43ae112ad4dd8a6841a6fbdac Mon Sep 17 00:00:00 2001 From: animeshsingh98 Date: Fri, 14 Feb 2025 17:15:53 -0600 Subject: [PATCH 56/71] Adding cruising speed profile --- .../onboard/planning/longitudinal_planning.py | 36 +++++++------------ 1 file changed, 12 insertions(+), 24 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index f851dea97..882bb89a3 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -99,17 +99,7 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentStat distance_to_move = pedestrian_back - vehicle_front - vehicle_buffer_x + time_to_pass * obj_v_y - #======replace this part with actual algorithm==== - - deceleration = relative_v ** 2 / (2 * distance) - if deceleration > max_deceleration: - return True, max_deceleration - if deceleration < min_deceleration: - return False, 0.0 - - return True, deceleration - - # ================================================ + return True, [distance_to_move, time_to_pass] else: deceleration = relative_v ** 2 / (2 * distance) @@ -266,9 +256,6 @@ def update(self, state : AllState): print("Lookahead distance:", lookahead_distance) route_to_end = route.trim(closest_parameter, len(route.points) - 1) - - #extract out a 10m segment of the route - # route_with_lookahead = route.trim(closest_parameter,closest_parameter+10.0) should_yield = False yield_deceleration = 0.0 @@ -277,19 +264,21 @@ def update(self, state : AllState): for r in state.relations: if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': - #yielding to something, brake - - #========================= - # print("#### YIELDING PLANNING ####") - #get the object we are yielding to obj = state.agents[r.obj2] detected, deceleration = detect_collision(abs_x, abs_y, curr_v, obj, self.min_deceleration, self.max_deceleration) - - if detected and deceleration > 0: - yield_deceleration = max(deceleration, yield_deceleration) - should_yield = True + if isinstance(deceleration, list): + time_collision = deceleration[1] + distance_collision = deceleration[0] + b = 3*time_collision - 2*curr_v + c = curr_v**2 - 3*distance_collision + self.desired_speed = (-b + (b**2 - 4*c)**0.5)/2 + self.deceleration = 1.5 + else: + if detected and deceleration > 0: + yield_deceleration = max(deceleration, yield_deceleration) + should_yield = True print("should yield: ", should_yield) @@ -298,5 +287,4 @@ def update(self, state : AllState): traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) else: traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v) - return traj From 450a2f8923265fbe0c66667f5aa57fbd199e3d2e Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Fri, 14 Feb 2025 17:23:07 -0600 Subject: [PATCH 57/71] Temporary fix of variables --- .../onboard/planning/longitudinal_planning.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 882bb89a3..8b0a772b6 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -154,8 +154,8 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m points = new_points - for p, t in zip(points, times): - print(f"Time: {t:.2f}, Point: {p}") + # for p, t in zip(points, times): + # print(f"Time: {t:.2f}, Point: {p}") trajectory = Trajectory(path.frame,points,times) return trajectory @@ -194,8 +194,8 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) new_points.append([x, 0]) points = new_points - for p, t in zip(points, times): - print(f"Time: {t:.2f}, Point: {p}") + # for p, t in zip(points, times): + # print(f"Time: {t:.2f}, Point: {p}") trajectory = Trajectory(path.frame,points,times) return trajectory @@ -273,8 +273,11 @@ def update(self, state : AllState): distance_collision = deceleration[0] b = 3*time_collision - 2*curr_v c = curr_v**2 - 3*distance_collision - self.desired_speed = (-b + (b**2 - 4*c)**0.5)/2 - self.deceleration = 1.5 + desired_speed = (-b + (b**2 - 4*c)**0.5)/2 + deceleration = 1.5 + print("@@@@@ YIELDING", desired_speed) + traj = longitudinal_plan(route_to_end, self.acceleration, deceleration, desired_speed, curr_v) + return traj else: if detected and deceleration > 0: yield_deceleration = max(deceleration, yield_deceleration) From b9f9e0ef3bd4b60c81313b9d5c39ac5a4218ea59 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Fri, 14 Feb 2025 19:07:12 -0600 Subject: [PATCH 58/71] Add condition that vehicle accels and ped walk to vehicle --- .../onboard/planning/longitudinal_planning.py | 20 +++++++++++++++++-- launch/pedestrian_detection.yaml | 2 -- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 23c36403c..f47d7d390 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -99,6 +99,19 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentStat distance_to_move = pedestrian_back - vehicle_front - vehicle_buffer_x + time_to_pass * obj_v_y + + # if curr_v**2/2*distance_to_pass >= 1.5: + # return True, curr_v**2/2*distance_to_pass + time_to_max_v = (5 - curr_v)/0.5 + + if time_to_max_v > time_to_pass: + if curr_v*time_to_pass + 0.25*time_to_pass**2 > distance_to_move: + return False, 0.0 + else: + if (25 - curr_v**2)*4 + (time_to_pass - time_to_max_v) * 5 >= distance_to_move: + return False, 0.0 + + return True, [distance_to_move, time_to_pass] else: @@ -503,6 +516,7 @@ def update(self, state : AllState): detected, deceleration = detect_collision(abs_x, abs_y, curr_v, obj, self.min_deceleration, self.max_deceleration) if isinstance(deceleration, list): + print("@@@@@ INPUT", deceleration) time_collision = deceleration[1] distance_collision = deceleration[0] b = 3*time_collision - 2*curr_v @@ -510,7 +524,8 @@ def update(self, state : AllState): desired_speed = (-b + (b**2 - 4*c)**0.5)/2 deceleration = 1.5 print("@@@@@ YIELDING", desired_speed) - traj = longitudinal_plan(route_to_end, self.acceleration, deceleration, desired_speed, curr_v) + route_with_lookahead = route.trim(closest_parameter,closest_parameter + distance_collision) + traj = longitudinal_plan(route_with_lookahead, self.acceleration, deceleration, desired_speed, curr_v) return traj else: if detected and deceleration > 0: @@ -523,5 +538,6 @@ def update(self, state : AllState): if should_yield: traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) else: - traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v) + # traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v) + traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v) return traj diff --git a/launch/pedestrian_detection.yaml b/launch/pedestrian_detection.yaml index be5bc8859..98f49a9aa 100644 --- a/launch/pedestrian_detection.yaml +++ b/launch/pedestrian_detection.yaml @@ -97,5 +97,3 @@ variants: # agent_detection : pedestrian_detection.FakePedestrianDetector2D agent_detection : OmniscientAgentDetector #this option reads agents from the simulator state_estimation : OmniscientStateEstimator - agent_detection : OmniscientAgentDetector - From 2872e78355bb1b6dac28b15a1324a14c85712971 Mon Sep 17 00:00:00 2001 From: animeshsingh98 Date: Fri, 14 Feb 2025 22:00:11 -0600 Subject: [PATCH 59/71] Optimising the program --- .../onboard/planning/longitudinal_planning.py | 488 +++++------------- 1 file changed, 130 insertions(+), 358 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index f47d7d390..f27f3690d 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -1,423 +1,208 @@ from typing import List, Tuple from ..component import Component -from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, ObjectFrameEnum, AgentState -from ...utils import serialization -from ...mathutils.transforms import vector_madd +from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, AgentState import time import numpy as np -import matplotlib.pyplot as plt -import matplotlib.patches as patches import math -def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float) -> Tuple[bool, float]: - """Detects if a collision will occur with the given object and return deceleration to avoid it.""" - - # Get the object's position and velocity - obj_x = obj.pose.x - obj_y = obj.pose.y - obj_v_x = obj.velocity[0] - obj_v_y = obj.velocity[1] - - pedestrian_length = 0.5 - pedestrian_width = 0.5 - - vehicle_length = 3.5 - vehicle_width = 2 - - vehicle_buffer_x = 3.0 - vehicle_buffer_y = 1.5 - - yield_buffer_y = 1.0 - - vehicle_front = curr_x + vehicle_length - vehicle_back = curr_x - vehicle_left = curr_y + vehicle_width / 2 - vehicle_right = curr_y - vehicle_width / 2 - - pedestrian_front = obj_x + pedestrian_length / 2 - pedestrian_back = obj_x - pedestrian_length / 2 - pedestrian_left = obj_y + pedestrian_width / 2 - pedestrian_right = obj_y - pedestrian_width / 2 - - # Check if the object is in front of the vehicle - if vehicle_front > pedestrian_back: - if vehicle_back > pedestrian_front: - # The object is behind the vehicle - print("Object is behind the vehicle") - return False, 0.0 - if vehicle_right - vehicle_buffer_y > pedestrian_left or vehicle_left + vehicle_buffer_y < pedestrian_right: - # The object is to the side of the vehicle - print("Object is to the side of the vehicle") - return False, 0.0 - # The object overlaps with the vehicle - return True, max_deceleration - - if vehicle_right - vehicle_buffer_y > pedestrian_left and obj_v_y <= 0: - # The object is to the right of the vehicle and moving away - print("Object is to the right of the vehicle and moving away") - return False, 0.0 - - if vehicle_left + vehicle_buffer_y < pedestrian_right and obj_v_y >= 0: - # The object is to the left of the vehicle and moving away - print("Object is to the left of the vehicle and moving away") +PEDESTRIAN_LENGTH = 0.5 +PEDESTRIAN_WIDTH = 0.5 + +VEHICLE_LENGTH = 3.5 +VEHICLE_WIDTH = 2 + +VEHICLE_BUFFER_X = 3.0 +VEHICLE_BUFFER_Y = 1.5 + +YIELD_BUFFER = 1.0 + +V_MAX = 5.0 +ACCEL_MAX = 0.5 + + +def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float) -> Tuple[bool, Union[float, List[float]]]: + """Detects potential collision and computes required deceleration or necessary movement parameters.""" + obj_x, obj_y = obj.pose.x, obj.pose.y + obj_v_x, obj_v_y = obj.velocity + + vehicle_front = curr_x + VEHICLE_LENGTH + vehicle_left, vehicle_right = curr_y + VEHICLE_WIDTH / 2, curr_y - VEHICLE_WIDTH / 2 + pedestrian_front, pedestrian_back = obj_x + PEDESTRIAN_LENGTH / 2, obj_x - PEDESTRIAN_LENGTH / 2 + pedestrian_left, pedestrian_right = obj_y + PEDESTRIAN_WIDTH / 2, obj_y - PEDESTRIAN_WIDTH / 2 + + # Object is behind or out of lateral bounds + if vehicle_front > pedestrian_back and vehicle_front - VEHICLE_BUFFER_X < pedestrian_back or \ + vehicle_right - VEHICLE_BUFFER_Y > pedestrian_left or vehicle_left + VEHICLE_BUFFER_Y < pedestrian_right: return False, 0.0 - - if vehicle_front + vehicle_buffer_x >= pedestrian_back: - # The object is in front of the vehicle and within the buffer - print("Object is in front of the vehicle and within the buffer") + + # Object is ahead and within collision zone + if vehicle_front + VEHICLE_BUFFER_X >= pedestrian_back: return True, max_deceleration - - # Calculate the deceleration needed to avoid a collision - print("Object is in front of the vehicle and outside the buffer") - distance = pedestrian_back - vehicle_front - vehicle_buffer_x + # Compute relative velocity relative_v = curr_v - obj_v_x if relative_v <= 0: return False, 0.0 - - print(relative_v, distance) - if obj_v_y > 0 and ((obj_y - curr_y) / relative_v) < ((vehicle_right - vehicle_buffer_y - yield_buffer_y - pedestrian_left) / abs(obj_v_y)): - # The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle - print("The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") - return False, 0.0 - if obj_v_y < 0 and ((obj_y - curr_y) / relative_v) < ((pedestrian_right - vehicle_left - vehicle_buffer_y - yield_buffer_y) / abs(obj_v_y)): - # The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle - print("The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") - return False, 0.0 - + # Handling object moving laterally if obj_v_y != 0: if obj_v_y < 0: - # The object is moving toward the right side of the vehicle - distance_to_pass = obj_y - (vehicle_right - vehicle_buffer_y - yield_buffer_y) + pedestrian_width / 2 - elif obj_v_y > 0: - # The object is moving toward the left side of the vehicle - distance_to_pass = (vehicle_left + vehicle_buffer_y + yield_buffer_y) - obj_y + pedestrian_width / 2 + distance_to_pass = obj_y - (vehicle_right - VEHICLE_BUFFER_Y - YIELD_BUFFER) + PEDESTRIAN_WIDTH / 2 + else: + distance_to_pass = (vehicle_left + VEHICLE_BUFFER_Y + YIELD_BUFFER) - obj_y + PEDESTRIAN_WIDTH / 2 time_to_pass = distance_to_pass / abs(obj_v_y) + distance_to_move = pedestrian_back - vehicle_front - VEHICLE_BUFFER_X + time_to_pass * obj_v_y - distance_to_move = pedestrian_back - vehicle_front - vehicle_buffer_x + time_to_pass * obj_v_y - - - # if curr_v**2/2*distance_to_pass >= 1.5: - # return True, curr_v**2/2*distance_to_pass - time_to_max_v = (5 - curr_v)/0.5 + time_to_max_v = (V_MAX - curr_v) / ACCEL_MAX if time_to_max_v > time_to_pass: - if curr_v*time_to_pass + 0.25*time_to_pass**2 > distance_to_move: + if curr_v * time_to_pass + 0.5 * ACCEL_MAX * time_to_pass**2 > distance_to_move: return False, 0.0 else: - if (25 - curr_v**2)*4 + (time_to_pass - time_to_max_v) * 5 >= distance_to_move: + if (V_MAX**2 - curr_v**2) / (2 * ACCEL_MAX) + (time_to_pass - time_to_max_v) * V_MAX >= distance_to_move: return False, 0.0 + if curr_v**2 / (2 * distance_to_pass) >= 1.5: + return True, curr_v**2 / (2 * distance_to_pass) return True, [distance_to_move, time_to_pass] - else: - deceleration = relative_v ** 2 / (2 * distance) - if deceleration > max_deceleration: - return True, max_deceleration - if deceleration < min_deceleration: - return False, 0.0 - - return True, deceleration - -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() + # Compute deceleration for avoiding collision + distance = pedestrian_back - vehicle_front - VEHICLE_BUFFER_X + deceleration = relative_v ** 2 / (2 * distance) - #TODO: actually do something to points and times - points = [p for p in path_normalized.points] - times = [t for t in path_normalized.times] - #============================================= + if deceleration > max_deceleration: + return True, max_deceleration + if deceleration < min_deceleration: + return False, 0.0 - print("-----LONGITUDINAL PLAN-----") - print("path length: ", path.length()) - length = path.length() + return True, deceleration - # If the path is too short, just return the path for preventing sudden halt of simulation - if length < 0.05: - return Trajectory(path.frame, points, times) - # This assumes that the time denomination cannot be changed +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.""" - # Starting point - x0 = points[0][0] - cur_point = points[0] - cur_time = times[0] - cur_index = 0 + path_normalized = path.arc_length_parameterize() + points = path_normalized.points + times = path_normalized.times - new_points = [] - new_times = [] - velocities = [] # for graphing and debugging purposes + path_length = path.length() + if path_length < 0.05: + return Trajectory(path.frame, points, times) - while current_speed > 0 or cur_index == 0: - # we want to iterate through all the points and add them - # to the new points. However, we also want to add "critical points" - # where we reach top speed, begin decelerating, and stop - new_points.append(cur_point) - new_times.append(cur_time) - velocities.append(current_speed) - print("=====================================") - # print("new points: ", new_points) - # print("current index: ", cur_index) - # print("current speed: ", current_speed) - # print("current position: ", cur_point) - - # Information we will need: - # Calculate how much time it would take to stop - # Calculate how much distance it would take to stop - min_delta_t_stop = current_speed/deceleration - min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 - # print(min_delta_x_stop) - assert min_delta_x_stop >= 0 - - - # Check if we are done - - # If we cannot stop before or stop exactly at the final position requested - if cur_point[0] + min_delta_x_stop >= points[-1][0]: - print("In case one") - # put on the breaks - - # Calculate the next point in a special manner because of too-little time to stop - if cur_index == len(points)-1: - # the next point in this instance would be when we stop - next_point = (cur_point[0] + min_delta_x_stop, 0) - else: - next_point = points[cur_index+1] + cur_point, cur_time, cur_index = points[0], times[0], 0 + new_points, new_times, velocities = [cur_point], [cur_time], [current_speed] - # keep breaking until the next milestone in path - if next_point[0] <= points[-1][0]: - # print("continuing to next point") - delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) - cur_time += delta_t_to_next_x - cur_point = next_point - current_speed -= deceleration*delta_t_to_next_x - cur_index += 1 - else: - # continue to the point in which we would stop (current_velocity = 0) - # update to the next point - delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) - cur_point = next_point - cur_time += delta_t_to_next_x - # current_speed would not be exactly zero error would be less than 1e-4 but perfer to just set to zero - #current_speed -= delta_t_to_next_x*deceleration - current_speed = 0 - assert current_speed == 0 - - # This is the case where we are accelerating to max speed - # because the first if-statement covers for when we decelerating, - # the only time current_speed < max_speed is when we are accelerating - elif current_speed < max_speed: - # print("In case two") - # next point - next_point = points[cur_index+1] - # accelerate to max speed - - # calculate the time it would take to reach max speed - delta_t_to_max_speed = (max_speed - current_speed)/acceleration - # calculate the distance it would take to reach max speed - delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 - - delta_t_to_stop_from_max_speed = max_speed/deceleration - delta_x_to_stop_from_max_speed = max_speed*delta_t_to_stop_from_max_speed - 0.5*deceleration*delta_t_to_stop_from_max_speed**2 - - delta_t_to_next_point = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) - velocity_at_next_point = current_speed + delta_t_to_next_point*acceleration - time_to_stop_from_next_point = velocity_at_next_point/deceleration - delta_x_to_stop_from_next_point = velocity_at_next_point*time_to_stop_from_next_point - 0.5*deceleration*time_to_stop_from_next_point**2 - # if we would reach max speed after the next point, - # just move to the next point and update the current speed and time - if next_point[0] + delta_x_to_stop_from_next_point < points[-1][0] and \ - cur_point[0] + delta_x_to_max_speed >= next_point[0]: - # ("go to next point") - # accelerate to max speed - delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) - cur_time += delta_t_to_next_x + while current_speed > 0 or cur_index == 0: + min_stop_time = current_speed / deceleration + min_stop_dist = current_speed * min_stop_time - 0.5 * deceleration * min_stop_time ** 2 + + if cur_point[0] + min_stop_dist >= points[-1][0]: # Need to decelerate + next_point = (cur_point[0] + min_stop_dist, 0) if cur_index == len(points) - 1 else points[cur_index + 1] + delta_t = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t + cur_point = next_point + current_speed = max(0, current_speed - deceleration * delta_t) + cur_index += 1 + + elif current_speed < max_speed: # Accelerating phase + next_point = points[cur_index + 1] + delta_t_max_speed = (max_speed - current_speed) / acceleration + delta_x_max_speed = current_speed * delta_t_max_speed + 0.5 * acceleration * delta_t_max_speed ** 2 + stop_x_from_max_speed = max_speed * (max_speed / deceleration) - 0.5 * deceleration * (max_speed / deceleration) ** 2 + + if next_point[0] + stop_x_from_max_speed < points[-1][0] and cur_point[0] + delta_x_max_speed >= next_point[0]: + delta_t = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + cur_time += delta_t cur_point = [next_point[0], 0] - current_speed += delta_t_to_next_x*acceleration + current_speed += delta_t * acceleration cur_index += 1 - - # This is the case where we would need to start breaking before reaching - # top speed and before the next point (i.e. triangle shape velocity) - elif cur_point[0] + delta_x_to_max_speed + delta_x_to_stop_from_max_speed >= points[-1][0]: - # print(delta_x_to_max_speed) - # print(delta_x_to_stop_from_max_speed) - # Add a new point at the point where we should start breaking - # print("Adding new point to start breaking") - delta_t_to_next_x = compute_time_triangle(cur_point[0], points[-1][0], current_speed, 0, acceleration, deceleration) - # print(delta_t_to_next_x) - #delta_t_to_next_x = compute_time_to_x(cur_point[0], points[-1][0] - min_delta_x_stop, current_speed, acceleration) - next_x = cur_point[0] + current_speed*delta_t_to_next_x + 0.5*acceleration*delta_t_to_next_x**2 - cur_time += delta_t_to_next_x - cur_point = [next_x, 0] - current_speed += delta_t_to_next_x*acceleration - - # this is the case where we would reach max speed before the next point - # we need to create a new point where we would reach max speed + + elif cur_point[0] + delta_x_max_speed + stop_x_from_max_speed >= points[-1][0]: + delta_t = compute_time_triangle(cur_point[0], points[-1][0], current_speed, 0, acceleration, deceleration) + cur_time += delta_t + cur_point = [cur_point[0] + current_speed * delta_t + 0.5 * acceleration * delta_t ** 2, 0] + current_speed += delta_t * acceleration + else: - # print("adding new point") - # we would need to add a new point at max speed - cur_time += delta_t_to_max_speed - cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + cur_time += delta_t_max_speed + cur_point = [cur_point[0] + delta_x_max_speed, 0] current_speed = max_speed - # This is the case where we are at max speed - # special functionality is that this block must - # add a point where we would need to start declerating to reach - # the final point - elif current_speed == max_speed: - next_point = points[cur_index+1] - # continue on with max speed - # print("In case three") - - # add point to start decelerating - if next_point[0] + min_delta_x_stop >= points[-1][0]: - # print("Adding new point to start decelerating") - cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed - cur_point = [points[-1][0] - min_delta_x_stop,0] - current_speed = max_speed + elif current_speed == max_speed: # Constant speed phase + next_point = points[cur_index + 1] + if next_point[0] + min_stop_dist >= points[-1][0]: + cur_time += (points[-1][0] - min_stop_dist - cur_point[0]) / current_speed + cur_point = [points[-1][0] - min_stop_dist, 0] else: - # Continue on to next point - # print("Continuing on to next point") - cur_time += (next_point[0] - cur_point[0])/current_speed + cur_time += (next_point[0] - cur_point[0]) / current_speed cur_point = next_point cur_index += 1 - - # This is an edge case and should only be reach - # if the initial speed is greater than the max speed - elif current_speed > max_speed: - # We need to hit the breaks - - next_point = points[cur_index+1] - # print("In case four") - # slow down to max speed - delta_t_to_max_speed = (current_speed - max_speed)/deceleration - delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 - - # If we would reach the next point before slowing down to max speed - # keep going until we reach the next point - if cur_point[0] + delta_x_to_max_speed >= next_point[0]: - delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) - cur_time += delta_t_to_next_x + + elif current_speed > max_speed: # Decelerating phase + next_point = points[cur_index + 1] + delta_t_max_speed = (current_speed - max_speed) / deceleration + delta_x_max_speed = current_speed * delta_t_max_speed - 0.5 * deceleration * delta_t_max_speed ** 2 + + if cur_point[0] + delta_x_max_speed >= next_point[0]: + delta_t = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t cur_point = [next_point[0], 0] - current_speed -= delta_t_to_next_x*deceleration + current_speed -= delta_t * deceleration cur_index += 1 else: - # We would reach max speed before the next point - # we need to add a new point at the point where we - # would reach max speed - cur_time += delta_t_to_max_speed - cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + cur_time += delta_t_max_speed + cur_point = [cur_point[0] + delta_x_max_speed, 0] current_speed = max_speed else: - # not sure what falls here - raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") - - new_points.append(cur_point) - new_times.append(cur_time) - velocities.append(current_speed) - - points = new_points - times = new_times - print("[PLAN] Computed points:", points) - print("[TIME] Computed time:", times) - print("[Velocities] Computed velocities:", velocities) + raise ValueError("Unexpected condition in longitudinal planning") + + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) - #============================================= + return Trajectory(path.frame, new_points, new_times, velocities) - trajectory = Trajectory(path.frame,points,times,velocities) - return trajectory -def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: +def compute_time_to_x(x0: float, x1: float, v: float, a: float) -> float: """Computes the time to go from x0 to x1 with initial velocity v0 and final velocity v1 - with constant acceleration a. I am assuming that we will always have a solution by settings + with constant acceleration a. I am assuming that we will always have a solution by setting discriminant equal to zero, i'm not sure if this is an issue.""" + return min(quad_root(a/2, v, x0 - x1)) - """Consider changing the system to use linear operators instead of explicitly calculating because of instances here""" - # x1 = x0 + v0*t + 0.5*a*t^2 - # x1 - x0 = v0*t + 0.5*a*t^2 - # 0.5*a*t^2 + v0*t + x0 - x1 = 0 - # t = (-v0 + sqrt(v0^2 - 4*0.5*a*(x0-x1)))/(2*0.5*a) - # t = (-v0 + sqrt(v0^2 + 2*a*(x1-x0)))/a - # print("x0: ", x0) - # print("x1: ", x1) - # print("v: ", v) - # print("a: ", a) - - t1 = (-v + max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a - t2 = (-v - max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a - - # print("t1: ", t1) - # print("t2: ", t2) - - if math.isnan(t1): t1 = 0 - if math.isnan(t2): t2 = 0 - - # print("t1: ", t1) - # print("t2: ", t2) - - valid_times = [n for n in [t1, t2] if n > 0] - if valid_times: - return min(valid_times) - else: - return 0.0 def compute_time_triangle(x0 : float, xf: float, v0: float, vf : float, acceleration : float, deceleration : float) -> float: """ Compute the time to go from current point assuming we are accelerating to the point at which we would need to start breaking in order to reach the final point with velocity 0.""" - # xf = v0*t1 + .5a*t1^2 + v1t2 -0.5d*t2^2 - # 0 = v1 - d*t2 = v0 + a*t1 - d * t2 - # t1 = (d*t2 - v0)/a - # xf = v0*(d*t2 - v0)/a + 0.5*a*(d*t2 - v0)^2/a^2 + v1*t2 - 0.5*d*t2^2 - # xf = v0*d*t2/a - v0^2/a + 0.5*a*(d*t2^2 - 2*v0*d*t2 + v0^2)/a^2 + v1*t2 - 0.5*d*t2^2 - # 0 = t2^2(0.5*a*d/a^2 - 0.5*d) + t2(v0*d/a - v0*d*a/a^2 + v1) -v0^2/a +0.5*a*v0^2/a^2 -xf roots = quad_root(0.5*acceleration + acceleration**2/deceleration - 0.5*acceleration**2/deceleration, v0+2*acceleration*v0/deceleration - acceleration*v0/deceleration, x0 - xf + v0**2/deceleration - 0.5*v0**2/deceleration) - print(roots) t1 = max(roots) assert t1 > 0 return t1 -def quad_root(a : float, b : float, c : float) -> float: - x1 = (-b + max(0,(b**2 - 4*a*c))**0.5)/(2*a) - x2 = (-b - max(0,(b**2 - 4*a*c))**0.5)/(2*a) - # print("t1: ", t1) - # print("t2: ", t2) +def quad_root(a: float, b: float, c: float) -> list[float]: + discriminant = b**2 - 4 * a * c + if discriminant < 0: + return [0.0, 0.0] # No real solution - if math.isnan(x1): x1 = 0 - if math.isnan(x2): x2 = 0 + sqrt_d = math.sqrt(discriminant) + t1, t2 = (-b + sqrt_d) / a, (-b - sqrt_d) / a - # print("t1: ", t1) - # print("t2: ", t2) + return [t1, t2] - valid_roots = [n for n in [x1, x2] if not type(n) is complex] - # print(f"Valid roots {valid_roots}") - return valid_roots 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] - #============================================= - print("=====LONGITUDINAL BRAKE=====") print("deceleration: ", deceleration) @@ -441,9 +226,6 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) new_points.append([x, 0]) points = new_points - # for p, t in zip(points, times): - # print(f"Time: {t:.2f}, Point: {p}") - trajectory = Trajectory(path.frame,points,times) return trajectory @@ -476,13 +258,12 @@ def rate(self): def update(self, state : AllState): start_time = time.time() - vehicle = state.vehicle # type: VehicleState - route = state.route # type: Route + vehicle = state.vehicle + route = state.route t = state.t if self.t_last is None: self.t_last = t - dt = t - self.t_last # Position in vehicle frame (Start (0,0) to (15,0)) curr_x = vehicle.pose.x @@ -492,7 +273,7 @@ def update(self, state : AllState): abs_x = curr_x + state.start_vehicle_pose.x abs_y = curr_y + state.start_vehicle_pose.y - #figure out where we are on the route + # 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]) @@ -500,30 +281,23 @@ def update(self, state : AllState): lookahead_distance = max(10, curr_v**2 / (2 * self.deceleration)) route_with_lookahead = route.trim(closest_parameter,closest_parameter + lookahead_distance) - print("Lookahead distance:", lookahead_distance) - - route_to_end = route.trim(closest_parameter, len(route.points) - 1) should_yield = False yield_deceleration = 0.0 - print("Current Speed: ", curr_v) - for r in state.relations: if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': - #get the object we are yielding to + # get the object we are yielding to obj = state.agents[r.obj2] detected, deceleration = detect_collision(abs_x, abs_y, curr_v, obj, self.min_deceleration, self.max_deceleration) if isinstance(deceleration, list): - print("@@@@@ INPUT", deceleration) time_collision = deceleration[1] distance_collision = deceleration[0] b = 3*time_collision - 2*curr_v c = curr_v**2 - 3*distance_collision desired_speed = (-b + (b**2 - 4*c)**0.5)/2 deceleration = 1.5 - print("@@@@@ YIELDING", desired_speed) route_with_lookahead = route.trim(closest_parameter,closest_parameter + distance_collision) traj = longitudinal_plan(route_with_lookahead, self.acceleration, deceleration, desired_speed, curr_v) return traj @@ -534,10 +308,8 @@ def update(self, state : AllState): print("should yield: ", should_yield) - #choose whether to accelerate, brake, or keep at current velocity if should_yield: traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) else: - # traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v) traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v) return traj From 00ddf95c80d0b4f77ff2bed75e0fc53bb113b253 Mon Sep 17 00:00:00 2001 From: Animesh Singh Date: Fri, 14 Feb 2025 22:07:47 -0600 Subject: [PATCH 60/71] Update longitudinal_planning.py --- GEMstack/onboard/planning/longitudinal_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index f27f3690d..3f1ed0a4a 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -1,4 +1,4 @@ -from typing import List, Tuple +from typing import List, Tuple, Union from ..component import Component from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, AgentState From 52e56c30b04b94e02eedb0cdf1ad855d7b5d4252 Mon Sep 17 00:00:00 2001 From: Hansheng Date: Sat, 15 Feb 2025 22:22:46 +0000 Subject: [PATCH 61/71] tmp --- GEMstack/onboard/planning/longitudinal_planning.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index a401efae8..f0b5e90fe 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -23,6 +23,8 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m return longitudinal_plan_milestone(path, acceleration, deceleration, max_speed, current_speed) elif method == "dt": return longitudinal_plan_dt(path, acceleration, deceleration, max_speed, current_speed) + elif method == "dx": + return longitudinal_plan_dx(path, acceleration, deceleration, max_speed, current_speed) else: raise NotImplementedError("Invalid method, only milestone and dt implemented.") @@ -468,6 +470,8 @@ def longitudinal_plan_dt(path, acceleration: float, deceleration: float, max_spe trajectory = Trajectory(path_norm.frame, points, list(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() From 4b89d5b66578f56eb900a8ce62d1a860bf61ac50 Mon Sep 17 00:00:00 2001 From: Hansheng Date: Sat, 15 Feb 2025 22:26:40 +0000 Subject: [PATCH 62/71] Add dx --- .../onboard/planning/longitudinal_planning.py | 208 +++++++++++++++++- 1 file changed, 207 insertions(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index f0b5e90fe..c0ba7d569 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -26,7 +26,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m elif method == "dx": return longitudinal_plan_dx(path, acceleration, deceleration, max_speed, current_speed) else: - raise NotImplementedError("Invalid method, only milestone and dt implemented.") + raise NotImplementedError("Invalid method, only milestone, dt, adn dx are implemented.") def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float) -> Trajectory: @@ -470,7 +470,213 @@ def longitudinal_plan_dt(path, acceleration: float, deceleration: float, max_spe trajectory = Trajectory(path_norm.frame, points, list(times)) return trajectory +def longitudinal_plan_dx(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] + #============================================= + # Adjust these two numbers to choose between computation speed or smoothness + rq = 0.1 # Smaller, smoother + multi = 5 # Larger, smoother + print("-----LONGITUDINAL PLAN-----") + print("path length: ", path.length()) + length = path.length() + + # If the path is too short, just return the path for preventing sudden halt of simulation + if length < 0.05: + return Trajectory(path.frame, points, times) + + # This assumes that the time denomination cannot be changed + + # Starting point + x0 = points[0][0] + cur_point = points[0] + cur_time = times[0] + cur_index = 0 + acc = 0 + + new_points = [] + new_times = [] + velocities = [] # for graphing and debugging purposes + + while current_speed > 0 or cur_index == 0: + # we want to iterate through all the points and add them + # to the new points. However, we also want to add "critical points" + # where we reach top speed, begin decelerating, and stop + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + print("=====================================") + print("new points: ", new_points) + print("current index: ", cur_index) + print("current speed: ", current_speed) + + # Information we will need: + # Calculate how much time it would take to stop + # Calculate how much distance it would take to stop + min_delta_t_stop = current_speed/deceleration + min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 + assert min_delta_x_stop >= 0 + + + # Check if we are done + + # If we cannot stop before or stop exactly at the final position requested + if cur_point[0] + min_delta_x_stop >= points[-1][0] - 0.0001: + acc = deceleration + flag = 1 + print("In case one") + # put on the breaks + # Calculate the next point in a special manner because of too-little time to stop + if cur_index >= len(points)-1: + # the next point in this instance would be when we stop + print(1) + if min_delta_x_stop < rq * acc: + next_point = (cur_point[0] + min_delta_x_stop, 0) + else: + next_point = (cur_point[0] + (min_delta_x_stop / (acc * multi)), 0) + flag = 0 + else: + print(2) + next_point = points[cur_index+1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + + # keep breaking until the next milestone in path + print("continuing to next point") + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = next_point + current_speed -= deceleration*delta_t_to_next_x + if flag: + cur_index += 1 + + # This is the case where we are accelerating to max speed + # because the first if-statement covers for when we decelerating, + # the only time current_speed < max_speed is when we are accelerating + elif current_speed < max_speed: + print("In case two") + print(current_speed) + acc = acceleration + flag = 1 + # next point + next_point = points[cur_index+1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + # accelerate to max speed + + # calculate the time it would take to reach max speed + delta_t_to_max_speed = (max_speed - current_speed)/acceleration + # calculate the distance it would take to reach max speed + delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 + + # if we would reach max speed after the next point, + # just move to the next point and update the current speed and time + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + print("go to next point") + # accelerate to max speed + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed += delta_t_to_next_x*acceleration + if flag: + cur_index += 1 + + # this is the case where we would reach max speed before the next point + # we need to create a new point where we would reach max speed + else: + print("adding new point") + # we would need to add a new point at max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + # This is the case where we are at max speed + # special functionality is that this block must + # add a point where we would need to start declerating to reach + # the final point + elif current_speed == max_speed: + next_point = points[cur_index+1] + # continue on with max speed + print("In case three") + + # add point to start decelerating + if next_point[0] + min_delta_x_stop >= points[-1][0]: + print("Adding new point to start decelerating") + cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed + cur_point = [points[-1][0] - min_delta_x_stop,0] + current_speed = max_speed + else: + # Continue on to next point + print("Continuing on to next point") + cur_time += (next_point[0] - cur_point[0])/current_speed + cur_point = next_point + cur_index += 1 + + # This is an edge case and should only be reach + # if the initial speed is greater than the max speed + elif current_speed > max_speed: + # We need to hit the breaks + acc = deceleration + flag = 1 + # next point + next_point = points[cur_index+1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + print("In case four") + # slow down to max speed + delta_t_to_max_speed = (current_speed - max_speed)/deceleration + delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 + + # If we would reach the next point before slowing down to max speed + # keep going until we reach the next point + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed -= delta_t_to_next_x*deceleration + cur_index += 1 + else: + # We would reach max speed before the next point + # we need to add a new point at the point where we + # would reach max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + else: + # not sure what falls here + raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") + + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + + points = new_points + times = new_times + print("[PLAN] Computed points:", points) + print("[TIME] Computed time:", times) + print("[Velocities] Computed velocities:", velocities) + + #============================================= + + 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.""" From 32336daa496d96f2976810aa9f64da9bbb547993 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Sun, 16 Feb 2025 10:47:19 -0600 Subject: [PATCH 63/71] Fixed syntax errors --- GEMstack/onboard/planning/longitudinal_planning.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 3f1ed0a4a..3f991f620 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -24,7 +24,7 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float) -> Tuple[bool, Union[float, List[float]]]: """Detects potential collision and computes required deceleration or necessary movement parameters.""" obj_x, obj_y = obj.pose.x, obj.pose.y - obj_v_x, obj_v_y = obj.velocity + obj_v_x, obj_v_y, _ = obj.velocity vehicle_front = curr_x + VEHICLE_LENGTH vehicle_left, vehicle_right = curr_y + VEHICLE_WIDTH / 2, curr_y - VEHICLE_WIDTH / 2 @@ -186,7 +186,7 @@ def compute_time_triangle(x0 : float, xf: float, v0: float, vf : float, acceler return t1 -def quad_root(a: float, b: float, c: float) -> list[float]: +def quad_root(a: float, b: float, c: float) -> Tuple[float,float]: discriminant = b**2 - 4 * a * c if discriminant < 0: return [0.0, 0.0] # No real solution From 503a403e05e46eb1d09706918ce06c9a146b2bf6 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 17 Feb 2025 09:39:38 -0600 Subject: [PATCH 64/71] Fixed ROS type typo --- GEMstack/onboard/interface/gem_hardware.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 3f2f26994..e13ff817e 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -186,7 +186,7 @@ def callback_with_gnss_reading(msg: INSNavGeod): ) speed = np.sqrt(msg.ve**2 + msg.vn**2) callback(GNSSReading(pose,speed,('error' if msg.error else 'ok'))) - self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading) + self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading) elif name == 'top_lidar': topic = self.ros_sensor_topics[name] if type is not None and (type is not PointCloud2 and type is not np.ndarray): From 30206809631011004979a7fae2967439bcf94161 Mon Sep 17 00:00:00 2001 From: animeshsingh98 Date: Mon, 17 Feb 2025 11:46:06 -0600 Subject: [PATCH 65/71] Optimising the program and adding commit by Kris --- GEMstack/onboard/interface/gem_hardware.py | 3 +- .../onboard/planning/longitudinal_planning.py | 145 +++++++++++------- 2 files changed, 92 insertions(+), 56 deletions(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 3f2f26994..fc6431f21 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -26,6 +26,7 @@ import numpy as np from ...utils import conversions + class GEMHardwareInterface(GEMInterface): """Interface for connnecting to the physical GEM e2 vehicle.""" def __init__(self): @@ -186,7 +187,7 @@ def callback_with_gnss_reading(msg: INSNavGeod): ) speed = np.sqrt(msg.ve**2 + msg.vn**2) callback(GNSSReading(pose,speed,('error' if msg.error else 'ok'))) - self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading) + self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading) elif name == 'top_lidar': topic = self.ros_sensor_topics[name] if type is not None and (type is not PointCloud2 and type is not np.ndarray): diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index e531963b9..ea2c75694 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -1,4 +1,4 @@ -from typing import List, Tuple, Union +from typing import List, Tuple from ..component import Component from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, AgentState @@ -25,68 +25,102 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float) -> Tuple[bool, Union[float, List[float]]]: """Detects potential collision and computes required deceleration or necessary movement parameters.""" obj_x, obj_y = obj.pose.x, obj.pose.y - obj_v_x, obj_v_y, _ = obj.velocity + obj_v_x, obj_v_y = obj.velocity vehicle_front = curr_x + VEHICLE_LENGTH + vehicle_back = curr_x vehicle_left, vehicle_right = curr_y + VEHICLE_WIDTH / 2, curr_y - VEHICLE_WIDTH / 2 pedestrian_front, pedestrian_back = obj_x + PEDESTRIAN_LENGTH / 2, obj_x - PEDESTRIAN_LENGTH / 2 pedestrian_left, pedestrian_right = obj_y + PEDESTRIAN_WIDTH / 2, obj_y - PEDESTRIAN_WIDTH / 2 - # Object is behind or out of lateral bounds - if vehicle_front > pedestrian_back and vehicle_front - VEHICLE_BUFFER_X < pedestrian_back or \ - vehicle_right - VEHICLE_BUFFER_Y > pedestrian_left or vehicle_left + VEHICLE_BUFFER_Y < pedestrian_right: + # Check if the object is in front of the vehicle + if vehicle_front > pedestrian_back: + if vehicle_back > pedestrian_front: + # The object is behind the vehicle + print("Object is behind the vehicle") + return False, 0.0 + if vehicle_right - VEHICLE_BUFFER_Y > pedestrian_left or vehicle_left + VEHICLE_BUFFER_Y < pedestrian_right: + # The object is to the side of the vehicle + print("Object is to the side of the vehicle") + return False, 0.0 + # The object overlaps with the vehicle + return True, max_deceleration + + if vehicle_right - VEHICLE_BUFFER_Y > pedestrian_left and obj_v_y <= 0: + # The object is to the right of the vehicle and moving away + print("Object is to the right of the vehicle and moving away") + return False, 0.0 + + if vehicle_left + VEHICLE_BUFFER_Y < pedestrian_right and obj_v_y >= 0: + # The object is to the left of the vehicle and moving away + print("Object is to the left of the vehicle and moving away") return False, 0.0 - # Object is ahead and within collision zone if vehicle_front + VEHICLE_BUFFER_X >= pedestrian_back: + # The object is in front of the vehicle and within the buffer + print("Object is in front of the vehicle and within the buffer") return True, max_deceleration - # Compute relative velocity + # Calculate the deceleration needed to avoid a collision + print("Object is in front of the vehicle and outside the buffer") + distance = pedestrian_back - vehicle_front - VEHICLE_BUFFER_X + relative_v = curr_v - obj_v_x if relative_v <= 0: return False, 0.0 - # Handling object moving laterally + print(relative_v, distance) + + if obj_v_y > 0 and ((obj_y - curr_y) / relative_v) < ((vehicle_right - VEHICLE_BUFFER_Y - YIELD_BUFFER - pedestrian_left) / abs(obj_v_y)): + # The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle + print("The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") + return False, 0.0 + if obj_v_y < 0 and ((obj_y - curr_y) / relative_v) < ((pedestrian_right - vehicle_left - VEHICLE_BUFFER_Y - YIELD_BUFFER) / abs(obj_v_y)): + # The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle + print("The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") + return False, 0.0 + if obj_v_y != 0: if obj_v_y < 0: + # The object is moving toward the right side of the vehicle distance_to_pass = obj_y - (vehicle_right - VEHICLE_BUFFER_Y - YIELD_BUFFER) + PEDESTRIAN_WIDTH / 2 else: + # The object is moving toward the left side of the vehicle distance_to_pass = (vehicle_left + VEHICLE_BUFFER_Y + YIELD_BUFFER) - obj_y + PEDESTRIAN_WIDTH / 2 time_to_pass = distance_to_pass / abs(obj_v_y) + distance_to_move = pedestrian_back - vehicle_front - VEHICLE_BUFFER_X + time_to_pass * obj_v_y - time_to_max_v = (V_MAX - curr_v) / ACCEL_MAX + + # if curr_v**2/2*distance_to_pass >= 1.5: + # return True, curr_v**2/2*distance_to_pass + time_to_max_v = (V_MAX - curr_v)/0.5 if time_to_max_v > time_to_pass: - if curr_v * time_to_pass + 0.5 * ACCEL_MAX * time_to_pass**2 > distance_to_move: + if curr_v*time_to_pass + 0.5*ACCEL_MAX*time_to_pass**2 > distance_to_move: return False, 0.0 else: - if (V_MAX**2 - curr_v**2) / (2 * ACCEL_MAX) + (time_to_pass - time_to_max_v) * V_MAX >= distance_to_move: + if (V_MAX**2 - curr_v**2)/(2*ACCEL_MAX) + (time_to_pass - time_to_max_v) * V_MAX >= distance_to_move: return False, 0.0 - if curr_v**2 / (2 * distance_to_pass) >= 1.5: - return True, curr_v**2 / (2 * distance_to_pass) - return True, [distance_to_move, time_to_pass] - # Compute deceleration for avoiding collision - distance = pedestrian_back - vehicle_front - VEHICLE_BUFFER_X - deceleration = relative_v ** 2 / (2 * distance) - - if deceleration > max_deceleration: - return True, max_deceleration - if deceleration < min_deceleration: - return False, 0.0 + else: + deceleration = relative_v ** 2 / (2 * distance) + if deceleration > max_deceleration: + return True, max_deceleration + if deceleration < min_deceleration: + return False, 0.0 - return True, deceleration + return True, deceleration def longitudinal_plan(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float, method : str) -> Trajectory: """Generates a longitudinal trajectory for a path with a - trapezoidal velocity profile. - + 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, @@ -101,12 +135,12 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m return longitudinal_plan_dx(path, acceleration, deceleration, max_speed, current_speed) else: raise NotImplementedError("Invalid method, only milestone, dt, adn dx are implemented.") - + def longitudinal_plan_milestone(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. - + 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, @@ -214,7 +248,7 @@ def compute_time_triangle(x0 : float, xf: float, v0: float, vf : float, acceler return t1 -def quad_root(a: float, b: float, c: float) -> Tuple[float,float]: +def quad_root(a: float, b: float, c: float) -> list[float]: discriminant = b**2 - 4 * a * c if discriminant < 0: return [0.0, 0.0] # No real solution @@ -226,7 +260,7 @@ def quad_root(a: float, b: float, c: float) -> Tuple[float,float]: def solve_for_v_peak(v0: float, acceleration: float, deceleration: float, total_length: float) -> float: - + if acceleration <= 0 or deceleration <= 0: raise ValueError("Acceleration and deceleration cant be negative") @@ -245,8 +279,8 @@ def compute_dynamic_dt(acceleration, speed, k=0.005, a_min=0.5): return np.sqrt(2 * position_step / max(acceleration, a_min)) def longitudinal_plan_dt(path, acceleration: float, deceleration: float, max_speed: float, current_speed: float): - - + + # 1 parametrizatiom. path_norm = path.arc_length_parameterize(speed=1.0) total_length = path.length() @@ -338,7 +372,7 @@ def longitudinal_plan_dt(path, acceleration: float, deceleration: float, max_spe # plt.draw() # plt.pause(0.001) - + # 4. Create a time grid. # dt = 0.1 # adjust based on computation @@ -383,8 +417,8 @@ def longitudinal_plan_dt(path, acceleration: float, deceleration: float, max_spe def longitudinal_plan_dx(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. - + 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, @@ -431,7 +465,7 @@ def longitudinal_plan_dx(path : Path, acceleration : float, deceleration : float print("current index: ", cur_index) print("current speed: ", current_speed) - # Information we will need: + # Information we will need: # Calculate how much time it would take to stop # Calculate how much distance it would take to stop min_delta_t_stop = current_speed/deceleration @@ -447,7 +481,7 @@ def longitudinal_plan_dx(path : Path, acceleration : float, deceleration : float flag = 1 print("In case one") # put on the breaks - # Calculate the next point in a special manner because of too-little time to stop + # Calculate the next point in a special manner because of too-little time to stop if cur_index >= len(points)-1: # the next point in this instance would be when we stop print(1) @@ -475,13 +509,13 @@ def longitudinal_plan_dx(path : Path, acceleration : float, deceleration : float # This is the case where we are accelerating to max speed # because the first if-statement covers for when we decelerating, - # the only time current_speed < max_speed is when we are accelerating + # the only time current_speed < max_speed is when we are accelerating elif current_speed < max_speed: print("In case two") print(current_speed) acc = acceleration flag = 1 - # next point + # next point next_point = points[cur_index+1] if next_point[0] - cur_point[0] > rq * acc: tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) @@ -493,8 +527,8 @@ def longitudinal_plan_dx(path : Path, acceleration : float, deceleration : float delta_t_to_max_speed = (max_speed - current_speed)/acceleration # calculate the distance it would take to reach max speed delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 - - # if we would reach max speed after the next point, + + # if we would reach max speed after the next point, # just move to the next point and update the current speed and time if cur_point[0] + delta_x_to_max_speed >= next_point[0]: print("go to next point") @@ -505,9 +539,9 @@ def longitudinal_plan_dx(path : Path, acceleration : float, deceleration : float current_speed += delta_t_to_next_x*acceleration if flag: cur_index += 1 - + # this is the case where we would reach max speed before the next point - # we need to create a new point where we would reach max speed + # we need to create a new point where we would reach max speed else: print("adding new point") # we would need to add a new point at max speed @@ -516,14 +550,14 @@ def longitudinal_plan_dx(path : Path, acceleration : float, deceleration : float current_speed = max_speed # This is the case where we are at max speed - # special functionality is that this block must + # special functionality is that this block must # add a point where we would need to start declerating to reach # the final point elif current_speed == max_speed: next_point = points[cur_index+1] # continue on with max speed print("In case three") - + # add point to start decelerating if next_point[0] + min_delta_x_stop >= points[-1][0]: print("Adding new point to start decelerating") @@ -536,21 +570,21 @@ def longitudinal_plan_dx(path : Path, acceleration : float, deceleration : float cur_time += (next_point[0] - cur_point[0])/current_speed cur_point = next_point cur_index += 1 - - # This is an edge case and should only be reach + + # This is an edge case and should only be reach # if the initial speed is greater than the max speed elif current_speed > max_speed: - # We need to hit the breaks + # We need to hit the breaks acc = deceleration flag = 1 - # next point + # next point next_point = points[cur_index+1] if next_point[0] - cur_point[0] > rq * acc: tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) flag = 0 next_point = [tmp, next_point[1]] print("In case four") - # slow down to max speed + # slow down to max speed delta_t_to_max_speed = (current_speed - max_speed)/deceleration delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 @@ -573,11 +607,11 @@ def longitudinal_plan_dx(path : Path, acceleration : float, deceleration : float else: # not sure what falls here raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") - + new_points.append(cur_point) new_times.append(cur_time) velocities.append(current_speed) - + points = new_points times = new_times print("[PLAN] Computed points:", points) @@ -627,6 +661,7 @@ class YieldTrajectoryPlanner(Component): 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 @@ -656,6 +691,7 @@ def update(self, state : AllState): if self.t_last is None: self.t_last = t + # Position in vehicle frame (Start (0,0) to (15,0)) curr_x = vehicle.pose.x curr_y = vehicle.pose.y curr_v = vehicle.v @@ -699,8 +735,7 @@ def update(self, state : AllState): print("should yield: ", should_yield) if should_yield: - traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) + traj = longitudinal_brake(route_to_end, yield_deceleration, curr_v) else: - traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v, "milestone") - + traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v) return traj From 0cac2681af97c919653ece16d0058fc4b9e45921 Mon Sep 17 00:00:00 2001 From: animeshsingh98 Date: Mon, 17 Feb 2025 11:46:06 -0600 Subject: [PATCH 66/71] Optimising the program and adding commit by Kris --- GEMstack/onboard/planning/longitudinal_planning.py | 1 + 1 file changed, 1 insertion(+) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index ea2c75694..d7bdf06ba 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -707,6 +707,7 @@ def update(self, state : AllState): lookahead_distance = max(10, curr_v**2 / (2 * self.deceleration)) route_with_lookahead = route.trim(closest_parameter,closest_parameter + lookahead_distance) + route_to_end = route.trim(closest_parameter, len(route.points) - 1) should_yield = False yield_deceleration = 0.0 From 0300ba900b5f8445cebe5615c9870bcbaf304a8a Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Mon, 17 Feb 2025 12:27:52 -0600 Subject: [PATCH 67/71] merge part1 longitudinal_plan and longitudinal_brake with part2 yeilding logic --- .../onboard/planning/longitudinal_planning.py | 818 +++++++++++++++--- scenes/xyhead_demo.yaml | 2 +- 2 files changed, 722 insertions(+), 98 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 882bb89a3..c68b963ec 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -10,6 +10,712 @@ import matplotlib.patches as patches import math +def longitudinal_plan(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float, + method : str) -> 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. + """ + + if method == "milestone": + return longitudinal_plan_milestone(path, acceleration, deceleration, max_speed, current_speed) + elif method == "dt": + return longitudinal_plan_dt(path, acceleration, deceleration, max_speed, current_speed) + elif method == "dx": + return longitudinal_plan_dx(path, acceleration, deceleration, max_speed, current_speed) + else: + raise NotImplementedError("Invalid method, only milestone, dt, adn dx are implemented.") + + +def longitudinal_plan_milestone(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] + #============================================= + + print("-----LONGITUDINAL PLAN-----") + print("path length: ", path.length()) + length = path.length() + + # If the path is too short, just return the path for preventing sudden halt of simulation + if length < 0.05: + return Trajectory(path.frame, points, times) + + # This assumes that the time denomination cannot be changed + + # Starting point + x0 = points[0][0] + cur_point = points[0] + cur_time = times[0] + cur_index = 0 + + new_points = [] + new_times = [] + velocities = [] # for graphing and debugging purposes + + while current_speed > 0 or cur_index == 0: + # we want to iterate through all the points and add them + # to the new points. However, we also want to add "critical points" + # where we reach top speed, begin decelerating, and stop + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + print("=====================================") + # print("new points: ", new_points) + # print("current index: ", cur_index) + # print("current speed: ", current_speed) + # print("current position: ", cur_point) + + # Information we will need: + # Calculate how much time it would take to stop + # Calculate how much distance it would take to stop + min_delta_t_stop = current_speed/deceleration + min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 + # print(min_delta_x_stop) + assert min_delta_x_stop >= 0 + + + # Check if we are done + + # If we cannot stop before or stop exactly at the final position requested + if cur_point[0] + min_delta_x_stop >= points[-1][0]: + print("In case one") + # put on the breaks + + # Calculate the next point in a special manner because of too-little time to stop + if cur_index == len(points)-1: + # the next point in this instance would be when we stop + next_point = (cur_point[0] + min_delta_x_stop, 0) + else: + next_point = points[cur_index+1] + + # keep breaking until the next milestone in path + if next_point[0] <= points[-1][0]: + # print("continuing to next point") + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = next_point + current_speed -= deceleration*delta_t_to_next_x + cur_index += 1 + else: + # continue to the point in which we would stop (current_velocity = 0) + # update to the next point + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_point = next_point + cur_time += delta_t_to_next_x + # current_speed would not be exactly zero error would be less than 1e-4 but perfer to just set to zero + #current_speed -= delta_t_to_next_x*deceleration + current_speed = 0 + assert current_speed == 0 + + # This is the case where we are accelerating to max speed + # because the first if-statement covers for when we decelerating, + # the only time current_speed < max_speed is when we are accelerating + elif current_speed < max_speed: + # print("In case two") + # next point + next_point = points[cur_index+1] + # accelerate to max speed + + # calculate the time it would take to reach max speed + delta_t_to_max_speed = (max_speed - current_speed)/acceleration + # calculate the distance it would take to reach max speed + delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 + + delta_t_to_stop_from_max_speed = max_speed/deceleration + delta_x_to_stop_from_max_speed = max_speed*delta_t_to_stop_from_max_speed - 0.5*deceleration*delta_t_to_stop_from_max_speed**2 + + delta_t_to_next_point = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + velocity_at_next_point = current_speed + delta_t_to_next_point*acceleration + time_to_stop_from_next_point = velocity_at_next_point/deceleration + delta_x_to_stop_from_next_point = velocity_at_next_point*time_to_stop_from_next_point - 0.5*deceleration*time_to_stop_from_next_point**2 + # if we would reach max speed after the next point, + # just move to the next point and update the current speed and time + if next_point[0] + delta_x_to_stop_from_next_point < points[-1][0] and \ + cur_point[0] + delta_x_to_max_speed >= next_point[0]: + # ("go to next point") + # accelerate to max speed + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed += delta_t_to_next_x*acceleration + cur_index += 1 + + # This is the case where we would need to start breaking before reaching + # top speed and before the next point (i.e. triangle shape velocity) + elif cur_point[0] + delta_x_to_max_speed + delta_x_to_stop_from_max_speed >= points[-1][0]: + # print(delta_x_to_max_speed) + # print(delta_x_to_stop_from_max_speed) + # Add a new point at the point where we should start breaking + # print("Adding new point to start breaking") + delta_t_to_next_x = compute_time_triangle(cur_point[0], points[-1][0], current_speed, 0, acceleration, deceleration) + # print(delta_t_to_next_x) + #delta_t_to_next_x = compute_time_to_x(cur_point[0], points[-1][0] - min_delta_x_stop, current_speed, acceleration) + next_x = cur_point[0] + current_speed*delta_t_to_next_x + 0.5*acceleration*delta_t_to_next_x**2 + cur_time += delta_t_to_next_x + cur_point = [next_x, 0] + current_speed += delta_t_to_next_x*acceleration + + # this is the case where we would reach max speed before the next point + # we need to create a new point where we would reach max speed + else: + # print("adding new point") + # we would need to add a new point at max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + # This is the case where we are at max speed + # special functionality is that this block must + # add a point where we would need to start declerating to reach + # the final point + elif current_speed == max_speed: + next_point = points[cur_index+1] + # continue on with max speed + # print("In case three") + + # add point to start decelerating + if next_point[0] + min_delta_x_stop >= points[-1][0]: + # print("Adding new point to start decelerating") + cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed + cur_point = [points[-1][0] - min_delta_x_stop,0] + current_speed = max_speed + else: + # Continue on to next point + # print("Continuing on to next point") + cur_time += (next_point[0] - cur_point[0])/current_speed + cur_point = next_point + cur_index += 1 + + # This is an edge case and should only be reach + # if the initial speed is greater than the max speed + elif current_speed > max_speed: + # We need to hit the breaks + + next_point = points[cur_index+1] + # print("In case four") + # slow down to max speed + delta_t_to_max_speed = (current_speed - max_speed)/deceleration + delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 + + # If we would reach the next point before slowing down to max speed + # keep going until we reach the next point + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed -= delta_t_to_next_x*deceleration + cur_index += 1 + else: + # We would reach max speed before the next point + # we need to add a new point at the point where we + # would reach max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + else: + # not sure what falls here + raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") + + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + + points = new_points + times = new_times + print("[PLAN] Computed points:", points) + print("[TIME] Computed time:", times) + print("[Velocities] Computed velocities:", velocities) + + #============================================= + + trajectory = Trajectory(path.frame,points,times) + return trajectory + +def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: + """Computes the time to go from x0 to x1 with initial velocity v0 and final velocity v1 + with constant acceleration a. I am assuming that we will always have a solution by settings + discriminant equal to zero, i'm not sure if this is an issue.""" + + """Consider changing the system to use linear operators instead of explicitly calculating because of instances here""" + # x1 = x0 + v0*t + 0.5*a*t^2 + # x1 - x0 = v0*t + 0.5*a*t^2 + # 0.5*a*t^2 + v0*t + x0 - x1 = 0 + # t = (-v0 + sqrt(v0^2 - 4*0.5*a*(x0-x1)))/(2*0.5*a) + # t = (-v0 + sqrt(v0^2 + 2*a*(x1-x0)))/a + # print("x0: ", x0) + # print("x1: ", x1) + # print("v: ", v) + # print("a: ", a) + + t1 = (-v + max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a + t2 = (-v - max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a + + # print("t1: ", t1) + # print("t2: ", t2) + + if math.isnan(t1): t1 = 0 + if math.isnan(t2): t2 = 0 + + # print("t1: ", t1) + # print("t2: ", t2) + + valid_times = [n for n in [t1, t2] if n > 0] + if valid_times: + return min(valid_times) + else: + return 0.0 + +def compute_time_triangle(x0 : float, xf: float, v0: float, vf : float, acceleration : float, deceleration : float) -> float: + """ + Compute the time to go from current point assuming we are accelerating to the point at which + we would need to start breaking in order to reach the final point with velocity 0.""" + # xf = v0*t1 + .5a*t1^2 + v1t2 -0.5d*t2^2 + # 0 = v1 - d*t2 = v0 + a*t1 - d * t2 + # t1 = (d*t2 - v0)/a + # xf = v0*(d*t2 - v0)/a + 0.5*a*(d*t2 - v0)^2/a^2 + v1*t2 - 0.5*d*t2^2 + # xf = v0*d*t2/a - v0^2/a + 0.5*a*(d*t2^2 - 2*v0*d*t2 + v0^2)/a^2 + v1*t2 - 0.5*d*t2^2 + # 0 = t2^2(0.5*a*d/a^2 - 0.5*d) + t2(v0*d/a - v0*d*a/a^2 + v1) -v0^2/a +0.5*a*v0^2/a^2 -xf + roots = quad_root(0.5*acceleration + acceleration**2/deceleration - 0.5*acceleration**2/deceleration, + v0+2*acceleration*v0/deceleration - acceleration*v0/deceleration, + x0 - xf + v0**2/deceleration - 0.5*v0**2/deceleration) + print(roots) + t1 = max(roots) + assert t1 > 0 + return t1 + +def quad_root(a : float, b : float, c : float) -> float: + x1 = (-b + max(0,(b**2 - 4*a*c))**0.5)/(2*a) + x2 = (-b - max(0,(b**2 - 4*a*c))**0.5)/(2*a) + + # print("t1: ", t1) + # print("t2: ", t2) + + if math.isnan(x1): x1 = 0 + if math.isnan(x2): x2 = 0 + + # print("t1: ", t1) + # print("t2: ", t2) + + valid_roots = [n for n in [x1, x2] if not type(n) is complex] + # print(f"Valid roots {valid_roots}") + return valid_roots + +def solve_for_v_peak(v0: float, acceleration: float, deceleration: float, total_length: float) -> float: + + if acceleration <= 0 or deceleration <= 0: + raise ValueError("Acceleration and deceleration cant be negative") + + #Formuala: (v_peak^2 - v0^2)/(2*a) + v_peak^2/(2*d) = total_length + numerator = deceleration * v0**2 + 2 * acceleration * deceleration * total_length + denominator = acceleration + deceleration + v_peak_sq = numerator / denominator + + if v_peak_sq < 0: + return 0.0 + + return math.sqrt(v_peak_sq) + +def compute_dynamic_dt(acceleration, speed, k=0.005, a_min=0.5): + position_step = k * max(speed, 1.0) # Ensures position step is speed-dependent + return np.sqrt(2 * position_step / max(acceleration, a_min)) + +def longitudinal_plan_dt(path, acceleration: float, deceleration: float, max_speed: float, current_speed: float): + + + # 1 parametrizatiom. + path_norm = path.arc_length_parameterize(speed=1.0) + total_length = path.length() + + # ------------------- + # If the path is too short, just return the path for preventing sudden halt of simulation + if total_length < 0.05: + points = [p for p in path_norm.points] + times = [t for t in path_norm.times] + return Trajectory(path.frame, points, times) + # ------------------- + + # 2. Compute distances for d_accel,d_decel + if max_speed > current_speed: + d_accel = (max_speed**2 - current_speed**2) / (2 * acceleration) + else: + d_accel = 0.0 # Already at or above max_speed + + d_decel = (max_speed**2) / (2 * deceleration) + + # 3. trapezoidal or triangle? + if d_accel + d_decel <= total_length: + t_accel = (max_speed - current_speed) / acceleration if max_speed > current_speed else 0.0 + t_decel = max_speed / deceleration + d_cruise = total_length - d_accel - d_decel + t_cruise = d_cruise / max_speed if max_speed != 0 else 0.0 + t_final = t_accel + t_cruise + t_decel + profile_type = "trapezoidal" + else: + # Triangular profile: not enough distance to reach max_speed so we will calculate peak speed. + peak_speed = solve_for_v_peak(current_speed, acceleration, deceleration, total_length) + # choose the min just in case + peak_speed = min(peak_speed, max_speed) + t_accel = (peak_speed - current_speed) / acceleration if peak_speed > current_speed else 0.0 + t_decel = peak_speed / deceleration + t_final = t_accel + t_decel + profile_type = "triangular" + + t = 0 + times = [] + s_vals = [] + num_time_steps = 0 + while t < t_final: + times.append(t) + if profile_type == "trapezoidal": + if t < t_accel: + # Acceleration phase. + s = current_speed * t + 0.5 * acceleration * t**2 + elif t < t_accel + t_cruise: + # Cruise phase. + s = d_accel + max_speed * (t - t_accel) + else: + # Deceleration phase. + t_decel_phase = t - (t_accel + t_cruise) + s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase)**2 + else: # Triangular profile. + if t < t_accel: + # Acceleration phase. + s = current_speed * t + 0.5 * acceleration * t**2 + else: + t_decel_phase = t - t_accel + s_accel = current_speed * t_accel + 0.5 * acceleration * t_accel**2 + s = s_accel + peak_speed * t_decel_phase - 0.5 * deceleration * t_decel_phase**2 + + s_vals.append(min(s, total_length)) + if s >= total_length: + break + + dt = compute_dynamic_dt(acceleration if t < t_accel else deceleration,current_speed) + t = t + dt + + num_time_steps +=1 + + # Compute trajectory points + points = [path_norm.eval(s) for s in s_vals] + print("Number of time steps is --------------------", num_time_steps) + + # return Trajectory(path_norm.frame, points, times) + + + # # Plot: update a single window + # import matplotlib.pyplot as plt + # plt.figure("Distance vs Time") + # plt.clf() # Clear the current figure + # plt.plot(times, s_vals) + # plt.xlabel("Time (s)") + # plt.ylabel("Distance (m)") + # plt.title("Distance vs Time") + # plt.draw() + # plt.pause(0.001) + + + + # 4. Create a time grid. + # dt = 0.1 # adjust based on computation + # times = np.arange(0, t_final + dt, dt) + # num_time_steps = 0 + + # # 5. Compute the distance s(t) for each time step. + # s_vals = [] + # for t in times: + # if profile_type == "trapezoidal": + # if t < t_accel: + # # Acceleration phase. + # s = current_speed * t + 0.5 * acceleration * t**2 + # elif t < t_accel + t_cruise: + # # Cruise phase. + # s = d_accel + max_speed * (t - t_accel) + # else: + # # Deceleration phase. + # t_decel_phase = t - (t_accel + t_cruise) + # # Compute the remaining distance using the deceleration equation. + # s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase)**2 + # else: # Triangular profile. + # if t < t_accel: + # # Acceleration phase. + # s = current_speed * t + 0.5 * acceleration * t**2 + # else: + # t_decel_phase = t - t_accel + # s_accel = current_speed * t_accel + 0.5 * acceleration * t_accel**2 + # s = s_accel + peak_speed * t_decel_phase - 0.5 * deceleration * t_decel_phase**2 + # num_time_steps +=1 + + # # should not exceed total path length + # s_vals.append(min(s, total_length)) + # print("NUmber of time steps -----------",num_time_steps) + # print("T FInal ----------------------------", t_final) + # points = [path_norm.eval(s) for s in s_vals] + + + + trajectory = Trajectory(path_norm.frame, points, list(times)) + return trajectory + +def longitudinal_plan_dx(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] + #============================================= + # Adjust these two numbers to choose between computation speed or smoothness + rq = 0.1 # Smaller, smoother + multi = 5 # Larger, smoother + print("-----LONGITUDINAL PLAN-----") + print("path length: ", path.length()) + length = path.length() + + # If the path is too short, just return the path for preventing sudden halt of simulation + if length < 0.05: + return Trajectory(path.frame, points, times) + + # This assumes that the time denomination cannot be changed + + # Starting point + x0 = points[0][0] + cur_point = points[0] + cur_time = times[0] + cur_index = 0 + acc = 0 + + new_points = [] + new_times = [] + velocities = [] # for graphing and debugging purposes + + while current_speed > 0 or cur_index == 0: + # we want to iterate through all the points and add them + # to the new points. However, we also want to add "critical points" + # where we reach top speed, begin decelerating, and stop + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + print("=====================================") + print("new points: ", new_points) + print("current index: ", cur_index) + print("current speed: ", current_speed) + + # Information we will need: + # Calculate how much time it would take to stop + # Calculate how much distance it would take to stop + min_delta_t_stop = current_speed/deceleration + min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 + assert min_delta_x_stop >= 0 + + + # Check if we are done + + # If we cannot stop before or stop exactly at the final position requested + if cur_point[0] + min_delta_x_stop >= points[-1][0] - 0.0001: + acc = deceleration + flag = 1 + print("In case one") + # put on the breaks + # Calculate the next point in a special manner because of too-little time to stop + if cur_index >= len(points)-1: + # the next point in this instance would be when we stop + print(1) + if min_delta_x_stop < rq * acc: + next_point = (cur_point[0] + min_delta_x_stop, 0) + else: + next_point = (cur_point[0] + (min_delta_x_stop / (acc * multi)), 0) + flag = 0 + else: + print(2) + next_point = points[cur_index+1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + + # keep breaking until the next milestone in path + print("continuing to next point") + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = next_point + current_speed -= deceleration*delta_t_to_next_x + if flag: + cur_index += 1 + + # This is the case where we are accelerating to max speed + # because the first if-statement covers for when we decelerating, + # the only time current_speed < max_speed is when we are accelerating + elif current_speed < max_speed: + print("In case two") + print(current_speed) + acc = acceleration + flag = 1 + # next point + next_point = points[cur_index+1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + # accelerate to max speed + + # calculate the time it would take to reach max speed + delta_t_to_max_speed = (max_speed - current_speed)/acceleration + # calculate the distance it would take to reach max speed + delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 + + # if we would reach max speed after the next point, + # just move to the next point and update the current speed and time + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + print("go to next point") + # accelerate to max speed + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed += delta_t_to_next_x*acceleration + if flag: + cur_index += 1 + + # this is the case where we would reach max speed before the next point + # we need to create a new point where we would reach max speed + else: + print("adding new point") + # we would need to add a new point at max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + # This is the case where we are at max speed + # special functionality is that this block must + # add a point where we would need to start declerating to reach + # the final point + elif current_speed == max_speed: + next_point = points[cur_index+1] + # continue on with max speed + print("In case three") + + # add point to start decelerating + if next_point[0] + min_delta_x_stop >= points[-1][0]: + print("Adding new point to start decelerating") + cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed + cur_point = [points[-1][0] - min_delta_x_stop,0] + current_speed = max_speed + else: + # Continue on to next point + print("Continuing on to next point") + cur_time += (next_point[0] - cur_point[0])/current_speed + cur_point = next_point + cur_index += 1 + + # This is an edge case and should only be reach + # if the initial speed is greater than the max speed + elif current_speed > max_speed: + # We need to hit the breaks + acc = deceleration + flag = 1 + # next point + next_point = points[cur_index+1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + print("In case four") + # slow down to max speed + delta_t_to_max_speed = (current_speed - max_speed)/deceleration + delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 + + # If we would reach the next point before slowing down to max speed + # keep going until we reach the next point + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed -= delta_t_to_next_x*deceleration + cur_index += 1 + else: + # We would reach max speed before the next point + # we need to add a new point at the point where we + # would reach max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + else: + # not sure what falls here + raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") + + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + + points = new_points + times = new_times + print("[PLAN] Computed points:", points) + print("[TIME] Computed time:", times) + print("[Velocities] Computed velocities:", velocities) + + #============================================= + + 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] + + #============================================= + + print("=====LONGITUDINAL BRAKE=====") + print("path length: ", path.length()) + length = path.length() + + x0 = points[0][0] + t_stop = current_speed / deceleration + x_stop = x0 + current_speed * t_stop - 0.5 * deceleration * t_stop**2 + + new_points = [] + velocities = [] + + for t in times: + if t <= t_stop: + x = x0 + current_speed * t - 0.5 * deceleration * t**2 + else: + x = x_stop + new_points.append([x, 0]) + velocities.append(current_speed - deceleration * t) + points = new_points + print("[BRAKE] Computed points:", points) + + #============================================= + + trajectory = Trajectory(path.frame,points,times) + return trajectory + def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float) -> Tuple[bool, float]: """Detects if a collision will occur with the given object and return deceleration to avoid it.""" @@ -110,97 +816,6 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentStat return True, deceleration -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] - - #============================================= - - print("-----LONGITUDINAL PLAN-----") - - look_ahead_time = 10.0 - dt = 0.2 - stop_distance_buffer = 0.2 - - trajectory_length = (look_ahead_time / dt) + 1 - times = np.linspace(0, look_ahead_time, int(trajectory_length)).tolist() - - new_points = [points[0]] - - for _ in range(1, len(times)): - distance_to_stop = current_speed **2 / (2 * deceleration) - if new_points[-1][0] >= points[-1][0]: - new_points.append([new_points[-1][0], 0]) - elif (distance_to_stop + stop_distance_buffer) >= (points[-1][0] - new_points[-1][0]): - # start decelerating - new_points.append([new_points[-1][0] + current_speed * dt - 0.5 * deceleration * dt**2, 0]) - current_speed -= deceleration * dt - elif current_speed < max_speed: - # start accelerating - new_points.append([new_points[-1][0] + current_speed * dt + 0.5 * acceleration * dt**2, 0]) - current_speed += acceleration * dt - else: - new_points.append([new_points[-1][0] + current_speed * dt, 0]) - - points = new_points - - for p, t in zip(points, times): - print(f"Time: {t:.2f}, Point: {p}") - - 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] - - #============================================= - - print("=====LONGITUDINAL BRAKE=====") - print("deceleration: ", deceleration) - - look_ahead_time = 10.0 - dt = 0.2 - - print(times[0]) - - trajectory_length = (look_ahead_time / dt) + 1 - times = np.linspace(0, look_ahead_time, int(trajectory_length)).tolist() - - stop_time = current_speed / deceleration - stop_distance = current_speed * stop_time / 2 - - new_points = [points[0]] - for i in range(1, len(times)): - if times[i] < stop_time: - x = points[0][0] + current_speed * times[i] - 0.5 * deceleration * times[i]**2 - else: - x = points[0][0] + stop_distance - new_points.append([x, 0]) - points = new_points - - for p, t in zip(points, times): - print(f"Time: {t:.2f}, Point: {p}") - - 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 @@ -211,7 +826,7 @@ def __init__(self): self.route_progress = None self.t_last = None self.acceleration = 0.5 - self.desired_speed = 1.0 + self.desired_speed = 2.0 self.deceleration = 2.0 self.min_deceleration = 1.0 @@ -269,12 +884,17 @@ def update(self, state : AllState): detected, deceleration = detect_collision(abs_x, abs_y, curr_v, obj, self.min_deceleration, self.max_deceleration) if isinstance(deceleration, list): + print("@@@@@ INPUT", deceleration) time_collision = deceleration[1] distance_collision = deceleration[0] b = 3*time_collision - 2*curr_v c = curr_v**2 - 3*distance_collision - self.desired_speed = (-b + (b**2 - 4*c)**0.5)/2 - self.deceleration = 1.5 + desired_speed = (-b + (b**2 - 4*c)**0.5)/2 + deceleration = 1.5 + print("@@@@@ YIELDING", desired_speed) + route_with_lookahead = route.trim(closest_parameter,closest_parameter + distance_collision) + traj = longitudinal_plan(route_to_end, self.acceleration, deceleration, desired_speed, curr_v, "dt") + return traj else: if detected and deceleration > 0: yield_deceleration = max(deceleration, yield_deceleration) @@ -282,9 +902,13 @@ def update(self, state : AllState): print("should yield: ", should_yield) + should_accelerate = (not should_yield and curr_v < self.desired_speed) + #choose whether to accelerate, brake, or keep at current velocity - if should_yield: - traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) + if should_accelerate: + traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v, "milestone") + elif should_yield: + traj = longitudinal_brake(route_to_end, yield_deceleration, curr_v) else: - traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v) + traj = longitudinal_plan(route_to_end, 0.0, self.deceleration, self.desired_speed, curr_v, "dt") return traj diff --git a/scenes/xyhead_demo.yaml b/scenes/xyhead_demo.yaml index c6d97477a..173724cf1 100644 --- a/scenes/xyhead_demo.yaml +++ b/scenes/xyhead_demo.yaml @@ -4,6 +4,6 @@ agents: type: pedestrian position: [15.0, 2.0] nominal_velocity: 0.5 - target: [15.0,10.0] + target: [15.0,30.0] behavior: loop \ No newline at end of file From e5a2232e0ba4de3f2f06b4cb90e1b777026b8716 Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Mon, 17 Feb 2025 12:33:22 -0600 Subject: [PATCH 68/71] Revert "merge part1 longitudinal_plan and longitudinal_brake with part2 yeilding logic" This reverts commit 0300ba900b5f8445cebe5615c9870bcbaf304a8a. --- .../onboard/planning/longitudinal_planning.py | 818 +++--------------- scenes/xyhead_demo.yaml | 2 +- 2 files changed, 98 insertions(+), 722 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 12984a26f..badf7f811 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -7,712 +7,6 @@ import math import numpy as np -def longitudinal_plan(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float, - method : str) -> 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. - """ - - if method == "milestone": - return longitudinal_plan_milestone(path, acceleration, deceleration, max_speed, current_speed) - elif method == "dt": - return longitudinal_plan_dt(path, acceleration, deceleration, max_speed, current_speed) - elif method == "dx": - return longitudinal_plan_dx(path, acceleration, deceleration, max_speed, current_speed) - else: - raise NotImplementedError("Invalid method, only milestone, dt, adn dx are implemented.") - - -def longitudinal_plan_milestone(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] - #============================================= - - print("-----LONGITUDINAL PLAN-----") - print("path length: ", path.length()) - length = path.length() - - # If the path is too short, just return the path for preventing sudden halt of simulation - if length < 0.05: - return Trajectory(path.frame, points, times) - - # This assumes that the time denomination cannot be changed - - # Starting point - x0 = points[0][0] - cur_point = points[0] - cur_time = times[0] - cur_index = 0 - - new_points = [] - new_times = [] - velocities = [] # for graphing and debugging purposes - - while current_speed > 0 or cur_index == 0: - # we want to iterate through all the points and add them - # to the new points. However, we also want to add "critical points" - # where we reach top speed, begin decelerating, and stop - new_points.append(cur_point) - new_times.append(cur_time) - velocities.append(current_speed) - print("=====================================") - # print("new points: ", new_points) - # print("current index: ", cur_index) - # print("current speed: ", current_speed) - # print("current position: ", cur_point) - - # Information we will need: - # Calculate how much time it would take to stop - # Calculate how much distance it would take to stop - min_delta_t_stop = current_speed/deceleration - min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 - # print(min_delta_x_stop) - assert min_delta_x_stop >= 0 - - - # Check if we are done - - # If we cannot stop before or stop exactly at the final position requested - if cur_point[0] + min_delta_x_stop >= points[-1][0]: - print("In case one") - # put on the breaks - - # Calculate the next point in a special manner because of too-little time to stop - if cur_index == len(points)-1: - # the next point in this instance would be when we stop - next_point = (cur_point[0] + min_delta_x_stop, 0) - else: - next_point = points[cur_index+1] - - # keep breaking until the next milestone in path - if next_point[0] <= points[-1][0]: - # print("continuing to next point") - delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) - cur_time += delta_t_to_next_x - cur_point = next_point - current_speed -= deceleration*delta_t_to_next_x - cur_index += 1 - else: - # continue to the point in which we would stop (current_velocity = 0) - # update to the next point - delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) - cur_point = next_point - cur_time += delta_t_to_next_x - # current_speed would not be exactly zero error would be less than 1e-4 but perfer to just set to zero - #current_speed -= delta_t_to_next_x*deceleration - current_speed = 0 - assert current_speed == 0 - - # This is the case where we are accelerating to max speed - # because the first if-statement covers for when we decelerating, - # the only time current_speed < max_speed is when we are accelerating - elif current_speed < max_speed: - # print("In case two") - # next point - next_point = points[cur_index+1] - # accelerate to max speed - - # calculate the time it would take to reach max speed - delta_t_to_max_speed = (max_speed - current_speed)/acceleration - # calculate the distance it would take to reach max speed - delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 - - delta_t_to_stop_from_max_speed = max_speed/deceleration - delta_x_to_stop_from_max_speed = max_speed*delta_t_to_stop_from_max_speed - 0.5*deceleration*delta_t_to_stop_from_max_speed**2 - - delta_t_to_next_point = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) - velocity_at_next_point = current_speed + delta_t_to_next_point*acceleration - time_to_stop_from_next_point = velocity_at_next_point/deceleration - delta_x_to_stop_from_next_point = velocity_at_next_point*time_to_stop_from_next_point - 0.5*deceleration*time_to_stop_from_next_point**2 - # if we would reach max speed after the next point, - # just move to the next point and update the current speed and time - if next_point[0] + delta_x_to_stop_from_next_point < points[-1][0] and \ - cur_point[0] + delta_x_to_max_speed >= next_point[0]: - # ("go to next point") - # accelerate to max speed - delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) - cur_time += delta_t_to_next_x - cur_point = [next_point[0], 0] - current_speed += delta_t_to_next_x*acceleration - cur_index += 1 - - # This is the case where we would need to start breaking before reaching - # top speed and before the next point (i.e. triangle shape velocity) - elif cur_point[0] + delta_x_to_max_speed + delta_x_to_stop_from_max_speed >= points[-1][0]: - # print(delta_x_to_max_speed) - # print(delta_x_to_stop_from_max_speed) - # Add a new point at the point where we should start breaking - # print("Adding new point to start breaking") - delta_t_to_next_x = compute_time_triangle(cur_point[0], points[-1][0], current_speed, 0, acceleration, deceleration) - # print(delta_t_to_next_x) - #delta_t_to_next_x = compute_time_to_x(cur_point[0], points[-1][0] - min_delta_x_stop, current_speed, acceleration) - next_x = cur_point[0] + current_speed*delta_t_to_next_x + 0.5*acceleration*delta_t_to_next_x**2 - cur_time += delta_t_to_next_x - cur_point = [next_x, 0] - current_speed += delta_t_to_next_x*acceleration - - # this is the case where we would reach max speed before the next point - # we need to create a new point where we would reach max speed - else: - # print("adding new point") - # we would need to add a new point at max speed - cur_time += delta_t_to_max_speed - cur_point = [cur_point[0] + delta_x_to_max_speed, 0] - current_speed = max_speed - - # This is the case where we are at max speed - # special functionality is that this block must - # add a point where we would need to start declerating to reach - # the final point - elif current_speed == max_speed: - next_point = points[cur_index+1] - # continue on with max speed - # print("In case three") - - # add point to start decelerating - if next_point[0] + min_delta_x_stop >= points[-1][0]: - # print("Adding new point to start decelerating") - cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed - cur_point = [points[-1][0] - min_delta_x_stop,0] - current_speed = max_speed - else: - # Continue on to next point - # print("Continuing on to next point") - cur_time += (next_point[0] - cur_point[0])/current_speed - cur_point = next_point - cur_index += 1 - - # This is an edge case and should only be reach - # if the initial speed is greater than the max speed - elif current_speed > max_speed: - # We need to hit the breaks - - next_point = points[cur_index+1] - # print("In case four") - # slow down to max speed - delta_t_to_max_speed = (current_speed - max_speed)/deceleration - delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 - - # If we would reach the next point before slowing down to max speed - # keep going until we reach the next point - if cur_point[0] + delta_x_to_max_speed >= next_point[0]: - delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) - cur_time += delta_t_to_next_x - cur_point = [next_point[0], 0] - current_speed -= delta_t_to_next_x*deceleration - cur_index += 1 - else: - # We would reach max speed before the next point - # we need to add a new point at the point where we - # would reach max speed - cur_time += delta_t_to_max_speed - cur_point = [cur_point[0] + delta_x_to_max_speed, 0] - current_speed = max_speed - - else: - # not sure what falls here - raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") - - new_points.append(cur_point) - new_times.append(cur_time) - velocities.append(current_speed) - - points = new_points - times = new_times - print("[PLAN] Computed points:", points) - print("[TIME] Computed time:", times) - print("[Velocities] Computed velocities:", velocities) - - #============================================= - - trajectory = Trajectory(path.frame,points,times) - return trajectory - -def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: - """Computes the time to go from x0 to x1 with initial velocity v0 and final velocity v1 - with constant acceleration a. I am assuming that we will always have a solution by settings - discriminant equal to zero, i'm not sure if this is an issue.""" - - """Consider changing the system to use linear operators instead of explicitly calculating because of instances here""" - # x1 = x0 + v0*t + 0.5*a*t^2 - # x1 - x0 = v0*t + 0.5*a*t^2 - # 0.5*a*t^2 + v0*t + x0 - x1 = 0 - # t = (-v0 + sqrt(v0^2 - 4*0.5*a*(x0-x1)))/(2*0.5*a) - # t = (-v0 + sqrt(v0^2 + 2*a*(x1-x0)))/a - # print("x0: ", x0) - # print("x1: ", x1) - # print("v: ", v) - # print("a: ", a) - - t1 = (-v + max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a - t2 = (-v - max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a - - # print("t1: ", t1) - # print("t2: ", t2) - - if math.isnan(t1): t1 = 0 - if math.isnan(t2): t2 = 0 - - # print("t1: ", t1) - # print("t2: ", t2) - - valid_times = [n for n in [t1, t2] if n > 0] - if valid_times: - return min(valid_times) - else: - return 0.0 - -def compute_time_triangle(x0 : float, xf: float, v0: float, vf : float, acceleration : float, deceleration : float) -> float: - """ - Compute the time to go from current point assuming we are accelerating to the point at which - we would need to start breaking in order to reach the final point with velocity 0.""" - # xf = v0*t1 + .5a*t1^2 + v1t2 -0.5d*t2^2 - # 0 = v1 - d*t2 = v0 + a*t1 - d * t2 - # t1 = (d*t2 - v0)/a - # xf = v0*(d*t2 - v0)/a + 0.5*a*(d*t2 - v0)^2/a^2 + v1*t2 - 0.5*d*t2^2 - # xf = v0*d*t2/a - v0^2/a + 0.5*a*(d*t2^2 - 2*v0*d*t2 + v0^2)/a^2 + v1*t2 - 0.5*d*t2^2 - # 0 = t2^2(0.5*a*d/a^2 - 0.5*d) + t2(v0*d/a - v0*d*a/a^2 + v1) -v0^2/a +0.5*a*v0^2/a^2 -xf - roots = quad_root(0.5*acceleration + acceleration**2/deceleration - 0.5*acceleration**2/deceleration, - v0+2*acceleration*v0/deceleration - acceleration*v0/deceleration, - x0 - xf + v0**2/deceleration - 0.5*v0**2/deceleration) - print(roots) - t1 = max(roots) - assert t1 > 0 - return t1 - -def quad_root(a : float, b : float, c : float) -> float: - x1 = (-b + max(0,(b**2 - 4*a*c))**0.5)/(2*a) - x2 = (-b - max(0,(b**2 - 4*a*c))**0.5)/(2*a) - - # print("t1: ", t1) - # print("t2: ", t2) - - if math.isnan(x1): x1 = 0 - if math.isnan(x2): x2 = 0 - - # print("t1: ", t1) - # print("t2: ", t2) - - valid_roots = [n for n in [x1, x2] if not type(n) is complex] - # print(f"Valid roots {valid_roots}") - return valid_roots - -def solve_for_v_peak(v0: float, acceleration: float, deceleration: float, total_length: float) -> float: - - if acceleration <= 0 or deceleration <= 0: - raise ValueError("Acceleration and deceleration cant be negative") - - #Formuala: (v_peak^2 - v0^2)/(2*a) + v_peak^2/(2*d) = total_length - numerator = deceleration * v0**2 + 2 * acceleration * deceleration * total_length - denominator = acceleration + deceleration - v_peak_sq = numerator / denominator - - if v_peak_sq < 0: - return 0.0 - - return math.sqrt(v_peak_sq) - -def compute_dynamic_dt(acceleration, speed, k=0.005, a_min=0.5): - position_step = k * max(speed, 1.0) # Ensures position step is speed-dependent - return np.sqrt(2 * position_step / max(acceleration, a_min)) - -def longitudinal_plan_dt(path, acceleration: float, deceleration: float, max_speed: float, current_speed: float): - - - # 1 parametrizatiom. - path_norm = path.arc_length_parameterize(speed=1.0) - total_length = path.length() - - # ------------------- - # If the path is too short, just return the path for preventing sudden halt of simulation - if total_length < 0.05: - points = [p for p in path_norm.points] - times = [t for t in path_norm.times] - return Trajectory(path.frame, points, times) - # ------------------- - - # 2. Compute distances for d_accel,d_decel - if max_speed > current_speed: - d_accel = (max_speed**2 - current_speed**2) / (2 * acceleration) - else: - d_accel = 0.0 # Already at or above max_speed - - d_decel = (max_speed**2) / (2 * deceleration) - - # 3. trapezoidal or triangle? - if d_accel + d_decel <= total_length: - t_accel = (max_speed - current_speed) / acceleration if max_speed > current_speed else 0.0 - t_decel = max_speed / deceleration - d_cruise = total_length - d_accel - d_decel - t_cruise = d_cruise / max_speed if max_speed != 0 else 0.0 - t_final = t_accel + t_cruise + t_decel - profile_type = "trapezoidal" - else: - # Triangular profile: not enough distance to reach max_speed so we will calculate peak speed. - peak_speed = solve_for_v_peak(current_speed, acceleration, deceleration, total_length) - # choose the min just in case - peak_speed = min(peak_speed, max_speed) - t_accel = (peak_speed - current_speed) / acceleration if peak_speed > current_speed else 0.0 - t_decel = peak_speed / deceleration - t_final = t_accel + t_decel - profile_type = "triangular" - - t = 0 - times = [] - s_vals = [] - num_time_steps = 0 - while t < t_final: - times.append(t) - if profile_type == "trapezoidal": - if t < t_accel: - # Acceleration phase. - s = current_speed * t + 0.5 * acceleration * t**2 - elif t < t_accel + t_cruise: - # Cruise phase. - s = d_accel + max_speed * (t - t_accel) - else: - # Deceleration phase. - t_decel_phase = t - (t_accel + t_cruise) - s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase)**2 - else: # Triangular profile. - if t < t_accel: - # Acceleration phase. - s = current_speed * t + 0.5 * acceleration * t**2 - else: - t_decel_phase = t - t_accel - s_accel = current_speed * t_accel + 0.5 * acceleration * t_accel**2 - s = s_accel + peak_speed * t_decel_phase - 0.5 * deceleration * t_decel_phase**2 - - s_vals.append(min(s, total_length)) - if s >= total_length: - break - - dt = compute_dynamic_dt(acceleration if t < t_accel else deceleration,current_speed) - t = t + dt - - num_time_steps +=1 - - # Compute trajectory points - points = [path_norm.eval(s) for s in s_vals] - print("Number of time steps is --------------------", num_time_steps) - - # return Trajectory(path_norm.frame, points, times) - - - # # Plot: update a single window - # import matplotlib.pyplot as plt - # plt.figure("Distance vs Time") - # plt.clf() # Clear the current figure - # plt.plot(times, s_vals) - # plt.xlabel("Time (s)") - # plt.ylabel("Distance (m)") - # plt.title("Distance vs Time") - # plt.draw() - # plt.pause(0.001) - - - - # 4. Create a time grid. - # dt = 0.1 # adjust based on computation - # times = np.arange(0, t_final + dt, dt) - # num_time_steps = 0 - - # # 5. Compute the distance s(t) for each time step. - # s_vals = [] - # for t in times: - # if profile_type == "trapezoidal": - # if t < t_accel: - # # Acceleration phase. - # s = current_speed * t + 0.5 * acceleration * t**2 - # elif t < t_accel + t_cruise: - # # Cruise phase. - # s = d_accel + max_speed * (t - t_accel) - # else: - # # Deceleration phase. - # t_decel_phase = t - (t_accel + t_cruise) - # # Compute the remaining distance using the deceleration equation. - # s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase)**2 - # else: # Triangular profile. - # if t < t_accel: - # # Acceleration phase. - # s = current_speed * t + 0.5 * acceleration * t**2 - # else: - # t_decel_phase = t - t_accel - # s_accel = current_speed * t_accel + 0.5 * acceleration * t_accel**2 - # s = s_accel + peak_speed * t_decel_phase - 0.5 * deceleration * t_decel_phase**2 - # num_time_steps +=1 - - # # should not exceed total path length - # s_vals.append(min(s, total_length)) - # print("NUmber of time steps -----------",num_time_steps) - # print("T FInal ----------------------------", t_final) - # points = [path_norm.eval(s) for s in s_vals] - - - - trajectory = Trajectory(path_norm.frame, points, list(times)) - return trajectory - -def longitudinal_plan_dx(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] - #============================================= - # Adjust these two numbers to choose between computation speed or smoothness - rq = 0.1 # Smaller, smoother - multi = 5 # Larger, smoother - print("-----LONGITUDINAL PLAN-----") - print("path length: ", path.length()) - length = path.length() - - # If the path is too short, just return the path for preventing sudden halt of simulation - if length < 0.05: - return Trajectory(path.frame, points, times) - - # This assumes that the time denomination cannot be changed - - # Starting point - x0 = points[0][0] - cur_point = points[0] - cur_time = times[0] - cur_index = 0 - acc = 0 - - new_points = [] - new_times = [] - velocities = [] # for graphing and debugging purposes - - while current_speed > 0 or cur_index == 0: - # we want to iterate through all the points and add them - # to the new points. However, we also want to add "critical points" - # where we reach top speed, begin decelerating, and stop - new_points.append(cur_point) - new_times.append(cur_time) - velocities.append(current_speed) - print("=====================================") - print("new points: ", new_points) - print("current index: ", cur_index) - print("current speed: ", current_speed) - - # Information we will need: - # Calculate how much time it would take to stop - # Calculate how much distance it would take to stop - min_delta_t_stop = current_speed/deceleration - min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 - assert min_delta_x_stop >= 0 - - - # Check if we are done - - # If we cannot stop before or stop exactly at the final position requested - if cur_point[0] + min_delta_x_stop >= points[-1][0] - 0.0001: - acc = deceleration - flag = 1 - print("In case one") - # put on the breaks - # Calculate the next point in a special manner because of too-little time to stop - if cur_index >= len(points)-1: - # the next point in this instance would be when we stop - print(1) - if min_delta_x_stop < rq * acc: - next_point = (cur_point[0] + min_delta_x_stop, 0) - else: - next_point = (cur_point[0] + (min_delta_x_stop / (acc * multi)), 0) - flag = 0 - else: - print(2) - next_point = points[cur_index+1] - if next_point[0] - cur_point[0] > rq * acc: - tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) - flag = 0 - next_point = [tmp, next_point[1]] - - # keep breaking until the next milestone in path - print("continuing to next point") - delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) - cur_time += delta_t_to_next_x - cur_point = next_point - current_speed -= deceleration*delta_t_to_next_x - if flag: - cur_index += 1 - - # This is the case where we are accelerating to max speed - # because the first if-statement covers for when we decelerating, - # the only time current_speed < max_speed is when we are accelerating - elif current_speed < max_speed: - print("In case two") - print(current_speed) - acc = acceleration - flag = 1 - # next point - next_point = points[cur_index+1] - if next_point[0] - cur_point[0] > rq * acc: - tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) - flag = 0 - next_point = [tmp, next_point[1]] - # accelerate to max speed - - # calculate the time it would take to reach max speed - delta_t_to_max_speed = (max_speed - current_speed)/acceleration - # calculate the distance it would take to reach max speed - delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 - - # if we would reach max speed after the next point, - # just move to the next point and update the current speed and time - if cur_point[0] + delta_x_to_max_speed >= next_point[0]: - print("go to next point") - # accelerate to max speed - delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) - cur_time += delta_t_to_next_x - cur_point = [next_point[0], 0] - current_speed += delta_t_to_next_x*acceleration - if flag: - cur_index += 1 - - # this is the case where we would reach max speed before the next point - # we need to create a new point where we would reach max speed - else: - print("adding new point") - # we would need to add a new point at max speed - cur_time += delta_t_to_max_speed - cur_point = [cur_point[0] + delta_x_to_max_speed, 0] - current_speed = max_speed - - # This is the case where we are at max speed - # special functionality is that this block must - # add a point where we would need to start declerating to reach - # the final point - elif current_speed == max_speed: - next_point = points[cur_index+1] - # continue on with max speed - print("In case three") - - # add point to start decelerating - if next_point[0] + min_delta_x_stop >= points[-1][0]: - print("Adding new point to start decelerating") - cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed - cur_point = [points[-1][0] - min_delta_x_stop,0] - current_speed = max_speed - else: - # Continue on to next point - print("Continuing on to next point") - cur_time += (next_point[0] - cur_point[0])/current_speed - cur_point = next_point - cur_index += 1 - - # This is an edge case and should only be reach - # if the initial speed is greater than the max speed - elif current_speed > max_speed: - # We need to hit the breaks - acc = deceleration - flag = 1 - # next point - next_point = points[cur_index+1] - if next_point[0] - cur_point[0] > rq * acc: - tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) - flag = 0 - next_point = [tmp, next_point[1]] - print("In case four") - # slow down to max speed - delta_t_to_max_speed = (current_speed - max_speed)/deceleration - delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 - - # If we would reach the next point before slowing down to max speed - # keep going until we reach the next point - if cur_point[0] + delta_x_to_max_speed >= next_point[0]: - delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) - cur_time += delta_t_to_next_x - cur_point = [next_point[0], 0] - current_speed -= delta_t_to_next_x*deceleration - cur_index += 1 - else: - # We would reach max speed before the next point - # we need to add a new point at the point where we - # would reach max speed - cur_time += delta_t_to_max_speed - cur_point = [cur_point[0] + delta_x_to_max_speed, 0] - current_speed = max_speed - - else: - # not sure what falls here - raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") - - new_points.append(cur_point) - new_times.append(cur_time) - velocities.append(current_speed) - - points = new_points - times = new_times - print("[PLAN] Computed points:", points) - print("[TIME] Computed time:", times) - print("[Velocities] Computed velocities:", velocities) - - #============================================= - - 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] - - #============================================= - - print("=====LONGITUDINAL BRAKE=====") - print("path length: ", path.length()) - length = path.length() - - x0 = points[0][0] - t_stop = current_speed / deceleration - x_stop = x0 + current_speed * t_stop - 0.5 * deceleration * t_stop**2 - - new_points = [] - velocities = [] - - for t in times: - if t <= t_stop: - x = x0 + current_speed * t - 0.5 * deceleration * t**2 - else: - x = x_stop - new_points.append([x, 0]) - velocities.append(current_speed - deceleration * t) - points = new_points - print("[BRAKE] Computed points:", points) - - #============================================= - - trajectory = Trajectory(path.frame,points,times) - return trajectory - def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float) -> Tuple[bool, float]: """Detects if a collision will occur with the given object and return deceleration to avoid it.""" @@ -827,6 +121,97 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentStat return True, deceleration +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] + + #============================================= + + print("-----LONGITUDINAL PLAN-----") + + look_ahead_time = 10.0 + dt = 0.2 + stop_distance_buffer = 0.2 + + trajectory_length = (look_ahead_time / dt) + 1 + times = np.linspace(0, look_ahead_time, int(trajectory_length)).tolist() + + new_points = [points[0]] + + for _ in range(1, len(times)): + distance_to_stop = current_speed **2 / (2 * deceleration) + if new_points[-1][0] >= points[-1][0]: + new_points.append([new_points[-1][0], 0]) + elif (distance_to_stop + stop_distance_buffer) >= (points[-1][0] - new_points[-1][0]): + # start decelerating + new_points.append([new_points[-1][0] + current_speed * dt - 0.5 * deceleration * dt**2, 0]) + current_speed -= deceleration * dt + elif current_speed < max_speed: + # start accelerating + new_points.append([new_points[-1][0] + current_speed * dt + 0.5 * acceleration * dt**2, 0]) + current_speed += acceleration * dt + else: + new_points.append([new_points[-1][0] + current_speed * dt, 0]) + + points = new_points + + for p, t in zip(points, times): + print(f"Time: {t:.2f}, Point: {p}") + + 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] + + #============================================= + + print("=====LONGITUDINAL BRAKE=====") + print("deceleration: ", deceleration) + + look_ahead_time = 10.0 + dt = 0.2 + + print(times[0]) + + trajectory_length = (look_ahead_time / dt) + 1 + times = np.linspace(0, look_ahead_time, int(trajectory_length)).tolist() + + stop_time = current_speed / deceleration + stop_distance = current_speed * stop_time / 2 + + new_points = [points[0]] + for i in range(1, len(times)): + if times[i] < stop_time: + x = points[0][0] + current_speed * times[i] - 0.5 * deceleration * times[i]**2 + else: + x = points[0][0] + stop_distance + new_points.append([x, 0]) + points = new_points + + for p, t in zip(points, times): + print(f"Time: {t:.2f}, Point: {p}") + + 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 @@ -837,7 +222,7 @@ def __init__(self): self.route_progress = None self.t_last = None self.acceleration = 0.5 - self.desired_speed = 2.0 + self.desired_speed = 1.0 self.deceleration = 2.0 self.min_deceleration = 1.0 @@ -890,17 +275,12 @@ def update(self, state : AllState): detected, deceleration = detect_collision(abs_x, abs_y, curr_v, obj, self.min_deceleration, self.max_deceleration) if isinstance(deceleration, list): - print("@@@@@ INPUT", deceleration) time_collision = deceleration[1] distance_collision = deceleration[0] b = 3*time_collision - 2*curr_v c = curr_v**2 - 3*distance_collision - desired_speed = (-b + (b**2 - 4*c)**0.5)/2 - deceleration = 1.5 - print("@@@@@ YIELDING", desired_speed) - route_with_lookahead = route.trim(closest_parameter,closest_parameter + distance_collision) - traj = longitudinal_plan(route_to_end, self.acceleration, deceleration, desired_speed, curr_v, "dt") - return traj + self.desired_speed = (-b + (b**2 - 4*c)**0.5)/2 + self.deceleration = 1.5 else: if detected and deceleration > 0: yield_deceleration = max(deceleration, yield_deceleration) @@ -908,13 +288,9 @@ def update(self, state : AllState): print("should yield: ", should_yield) - should_accelerate = (not should_yield and curr_v < self.desired_speed) - #choose whether to accelerate, brake, or keep at current velocity - if should_accelerate: - traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v, "milestone") - elif should_yield: - traj = longitudinal_brake(route_to_end, yield_deceleration, curr_v) + if should_yield: + traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) else: - traj = longitudinal_plan(route_to_end, 0.0, self.deceleration, self.desired_speed, curr_v, "dt") + traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v) return traj diff --git a/scenes/xyhead_demo.yaml b/scenes/xyhead_demo.yaml index 173724cf1..c6d97477a 100644 --- a/scenes/xyhead_demo.yaml +++ b/scenes/xyhead_demo.yaml @@ -4,6 +4,6 @@ agents: type: pedestrian position: [15.0, 2.0] nominal_velocity: 0.5 - target: [15.0,30.0] + target: [15.0,10.0] behavior: loop \ No newline at end of file From bca62337a7bc7b0373cedf17be5475af14ccc31c Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Mon, 17 Feb 2025 13:14:37 -0600 Subject: [PATCH 69/71] merge part1 and part2 --- .../onboard/planning/longitudinal_planning.py | 1224 +++++++++++++---- 1 file changed, 928 insertions(+), 296 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index badf7f811..4e59a9e0f 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -1,296 +1,928 @@ -from typing import List, Tuple -from ..component import Component -from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, AgentState - -import time -import numpy as np -import math -import numpy as np - -def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float) -> Tuple[bool, float]: - """Detects if a collision will occur with the given object and return deceleration to avoid it.""" - - # Get the object's position and velocity - obj_x = obj.pose.x - obj_y = obj.pose.y - obj_v_x = obj.velocity[0] - obj_v_y = obj.velocity[1] - -VEHICLE_LENGTH = 3.5 -VEHICLE_WIDTH = 2 - -VEHICLE_BUFFER_X = 3.0 -VEHICLE_BUFFER_Y = 1.5 - -YIELD_BUFFER = 1.0 - -V_MAX = 5.0 -ACCEL_MAX = 0.5 - - -def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float) -> Tuple[bool, Union[float, List[float]]]: - """Detects potential collision and computes required deceleration or necessary movement parameters.""" - obj_x, obj_y = obj.pose.x, obj.pose.y - obj_v_x, obj_v_y = obj.velocity - - vehicle_front = curr_x + VEHICLE_LENGTH - vehicle_back = curr_x - vehicle_left, vehicle_right = curr_y + VEHICLE_WIDTH / 2, curr_y - VEHICLE_WIDTH / 2 - pedestrian_front, pedestrian_back = obj_x + PEDESTRIAN_LENGTH / 2, obj_x - PEDESTRIAN_LENGTH / 2 - pedestrian_left, pedestrian_right = obj_y + PEDESTRIAN_WIDTH / 2, obj_y - PEDESTRIAN_WIDTH / 2 - - # Check if the object is in front of the vehicle - if vehicle_front > pedestrian_back: - if vehicle_back > pedestrian_front: - # The object is behind the vehicle - print("Object is behind the vehicle") - return False, 0.0 - if vehicle_right - VEHICLE_BUFFER_Y > pedestrian_left or vehicle_left + VEHICLE_BUFFER_Y < pedestrian_right: - # The object is to the side of the vehicle - print("Object is to the side of the vehicle") - return False, 0.0 - # The object overlaps with the vehicle - return True, max_deceleration - - if vehicle_right - VEHICLE_BUFFER_Y > pedestrian_left and obj_v_y <= 0: - # The object is to the right of the vehicle and moving away - print("Object is to the right of the vehicle and moving away") - return False, 0.0 - - if vehicle_left + VEHICLE_BUFFER_Y < pedestrian_right and obj_v_y >= 0: - # The object is to the left of the vehicle and moving away - print("Object is to the left of the vehicle and moving away") - return False, 0.0 - - if vehicle_front + VEHICLE_BUFFER_X >= pedestrian_back: - # The object is in front of the vehicle and within the buffer - print("Object is in front of the vehicle and within the buffer") - return True, max_deceleration - - # Calculate the deceleration needed to avoid a collision - print("Object is in front of the vehicle and outside the buffer") - distance = pedestrian_back - vehicle_front - VEHICLE_BUFFER_X - - relative_v = curr_v - obj_v_x - if relative_v <= 0: - return False, 0.0 - - print(relative_v, distance) - - if obj_v_y > 0 and ((obj_y - curr_y) / relative_v) < ((vehicle_right - VEHICLE_BUFFER_Y - YIELD_BUFFER - pedestrian_left) / abs(obj_v_y)): - # The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle - print("The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") - return False, 0.0 - if obj_v_y < 0 and ((obj_y - curr_y) / relative_v) < ((pedestrian_right - vehicle_left - VEHICLE_BUFFER_Y - YIELD_BUFFER) / abs(obj_v_y)): - # The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle - print("The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") - return False, 0.0 - - if obj_v_y != 0: - if obj_v_y < 0: - # The object is moving toward the right side of the vehicle - distance_to_pass = obj_y - (vehicle_right - VEHICLE_BUFFER_Y - YIELD_BUFFER) + PEDESTRIAN_WIDTH / 2 - else: - # The object is moving toward the left side of the vehicle - distance_to_pass = (vehicle_left + VEHICLE_BUFFER_Y + YIELD_BUFFER) - obj_y + PEDESTRIAN_WIDTH / 2 - - time_to_pass = distance_to_pass / abs(obj_v_y) - - distance_to_move = pedestrian_back - vehicle_front - VEHICLE_BUFFER_X + time_to_pass * obj_v_y - - - # if curr_v**2/2*distance_to_pass >= 1.5: - # return True, curr_v**2/2*distance_to_pass - time_to_max_v = (V_MAX - curr_v)/0.5 - - if time_to_max_v > time_to_pass: - if curr_v*time_to_pass + 0.5*ACCEL_MAX*time_to_pass**2 > distance_to_move: - return False, 0.0 - else: - if (V_MAX**2 - curr_v**2)/(2*ACCEL_MAX) + (time_to_pass - time_to_max_v) * V_MAX >= distance_to_move: - return False, 0.0 - - return True, [distance_to_move, time_to_pass] - - else: - deceleration = relative_v ** 2 / (2 * distance) - if deceleration > max_deceleration: - return True, max_deceleration - if deceleration < min_deceleration: - return False, 0.0 - - return True, deceleration - -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] - - #============================================= - - print("-----LONGITUDINAL PLAN-----") - - look_ahead_time = 10.0 - dt = 0.2 - stop_distance_buffer = 0.2 - - trajectory_length = (look_ahead_time / dt) + 1 - times = np.linspace(0, look_ahead_time, int(trajectory_length)).tolist() - - new_points = [points[0]] - - for _ in range(1, len(times)): - distance_to_stop = current_speed **2 / (2 * deceleration) - if new_points[-1][0] >= points[-1][0]: - new_points.append([new_points[-1][0], 0]) - elif (distance_to_stop + stop_distance_buffer) >= (points[-1][0] - new_points[-1][0]): - # start decelerating - new_points.append([new_points[-1][0] + current_speed * dt - 0.5 * deceleration * dt**2, 0]) - current_speed -= deceleration * dt - elif current_speed < max_speed: - # start accelerating - new_points.append([new_points[-1][0] + current_speed * dt + 0.5 * acceleration * dt**2, 0]) - current_speed += acceleration * dt - else: - new_points.append([new_points[-1][0] + current_speed * dt, 0]) - - points = new_points - - for p, t in zip(points, times): - print(f"Time: {t:.2f}, Point: {p}") - - 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] - - #============================================= - - print("=====LONGITUDINAL BRAKE=====") - print("deceleration: ", deceleration) - - look_ahead_time = 10.0 - dt = 0.2 - - print(times[0]) - - trajectory_length = (look_ahead_time / dt) + 1 - times = np.linspace(0, look_ahead_time, int(trajectory_length)).tolist() - - stop_time = current_speed / deceleration - stop_distance = current_speed * stop_time / 2 - - new_points = [points[0]] - for i in range(1, len(times)): - if times[i] < stop_time: - x = points[0][0] + current_speed * times[i] - 0.5 * deceleration * times[i]**2 - else: - x = points[0][0] + stop_distance - new_points.append([x, 0]) - points = new_points - - for p, t in zip(points, times): - print(f"Time: {t:.2f}, Point: {p}") - - 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 - - self.min_deceleration = 1.0 - self.max_deceleration = 8.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): - start_time = time.time() - - vehicle = state.vehicle - route = state.route - t = state.t - - if self.t_last is None: - self.t_last = t - - # Position in vehicle frame (Start (0,0) to (15,0)) - curr_x = vehicle.pose.x - curr_y = vehicle.pose.y - curr_v = vehicle.v - - abs_x = curr_x + state.start_vehicle_pose.x - abs_y = curr_y + state.start_vehicle_pose.y - - # 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 - - lookahead_distance = max(10, curr_v**2 / (2 * self.deceleration)) - route_with_lookahead = route.trim(closest_parameter,closest_parameter + lookahead_distance) - route_to_end = route.trim(closest_parameter, len(route.points) - 1) - - should_yield = False - yield_deceleration = 0.0 - - for r in state.relations: - if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': - # get the object we are yielding to - obj = state.agents[r.obj2] - - detected, deceleration = detect_collision(abs_x, abs_y, curr_v, obj, self.min_deceleration, self.max_deceleration) - if isinstance(deceleration, list): - time_collision = deceleration[1] - distance_collision = deceleration[0] - b = 3*time_collision - 2*curr_v - c = curr_v**2 - 3*distance_collision - self.desired_speed = (-b + (b**2 - 4*c)**0.5)/2 - self.deceleration = 1.5 - else: - if detected and deceleration > 0: - yield_deceleration = max(deceleration, yield_deceleration) - should_yield = True - - print("should yield: ", should_yield) - - #choose whether to accelerate, brake, or keep at current velocity - if should_yield: - traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) - else: - traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v) - return traj +from typing import List, Tuple +from ..component import Component +from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, ObjectFrameEnum, AgentState +from ...utils import serialization +from ...mathutils.transforms import vector_madd + +import time +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.patches as patches +import math + +def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float) -> Tuple[bool, float]: + """Detects if a collision will occur with the given object and return deceleration to avoid it.""" + + # Get the object's position and velocity + obj_x = obj.pose.x + obj_y = obj.pose.y + obj_v_x = obj.velocity[0] + obj_v_y = obj.velocity[1] + + pedestrian_length = 0.5 + pedestrian_width = 0.5 + + vehicle_length = 3.5 + vehicle_width = 2 + + vehicle_buffer_x = 3.0 + vehicle_buffer_y = 1.5 + + yield_buffer_y = 1.0 + + vehicle_front = curr_x + vehicle_length + vehicle_back = curr_x + vehicle_left = curr_y + vehicle_width / 2 + vehicle_right = curr_y - vehicle_width / 2 + + pedestrian_front = obj_x + pedestrian_length / 2 + pedestrian_back = obj_x - pedestrian_length / 2 + pedestrian_left = obj_y + pedestrian_width / 2 + pedestrian_right = obj_y - pedestrian_width / 2 + + # Check if the object is in front of the vehicle + if vehicle_front > pedestrian_back: + if vehicle_back > pedestrian_front: + # The object is behind the vehicle + print("Object is behind the vehicle") + return False, 0.0 + if vehicle_right - vehicle_buffer_y > pedestrian_left or vehicle_left + vehicle_buffer_y < pedestrian_right: + # The object is to the side of the vehicle + print("Object is to the side of the vehicle") + return False, 0.0 + # The object overlaps with the vehicle + return True, max_deceleration + + if vehicle_right - vehicle_buffer_y > pedestrian_left and obj_v_y <= 0: + # The object is to the right of the vehicle and moving away + print("Object is to the right of the vehicle and moving away") + return False, 0.0 + + if vehicle_left + vehicle_buffer_y < pedestrian_right and obj_v_y >= 0: + # The object is to the left of the vehicle and moving away + print("Object is to the left of the vehicle and moving away") + return False, 0.0 + + if vehicle_front + vehicle_buffer_x >= pedestrian_back: + # The object is in front of the vehicle and within the buffer + print("Object is in front of the vehicle and within the buffer") + return True, max_deceleration + + # Calculate the deceleration needed to avoid a collision + print("Object is in front of the vehicle and outside the buffer") + distance = pedestrian_back - vehicle_front - vehicle_buffer_x + + relative_v = curr_v - obj_v_x + if relative_v <= 0: + return False, 0.0 + + print(relative_v, distance) + + if obj_v_y > 0 and ((obj_y - curr_y) / relative_v) < ((vehicle_right - vehicle_buffer_y - yield_buffer_y - pedestrian_left) / abs(obj_v_y)): + # The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle + print("The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") + return False, 0.0 + if obj_v_y < 0 and ((obj_y - curr_y) / relative_v) < ((pedestrian_right - vehicle_left - vehicle_buffer_y - yield_buffer_y) / abs(obj_v_y)): + # The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle + print("The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") + return False, 0.0 + + if obj_v_y != 0: + if obj_v_y < 0: + # The object is moving toward the right side of the vehicle + distance_to_pass = obj_y - (vehicle_right - vehicle_buffer_y - yield_buffer_y) + pedestrian_width / 2 + elif obj_v_y > 0: + # The object is moving toward the left side of the vehicle + distance_to_pass = (vehicle_left + vehicle_buffer_y + yield_buffer_y) - obj_y + pedestrian_width / 2 + + time_to_pass = distance_to_pass / abs(obj_v_y) + + distance_to_move = pedestrian_back - vehicle_front - vehicle_buffer_x + time_to_pass * obj_v_y + + + # if curr_v**2/2*distance_to_pass >= 1.5: + # return True, curr_v**2/2*distance_to_pass + time_to_max_v = (5 - curr_v)/0.5 + + if time_to_max_v > time_to_pass: + if curr_v*time_to_pass + 0.25*time_to_pass**2 > distance_to_move: + return False, 0.0 + else: + if (25 - curr_v**2)*4 + (time_to_pass - time_to_max_v) * 5 >= distance_to_move: + return False, 0.0 + + + return True, [distance_to_move, time_to_pass] + + else: + deceleration = relative_v ** 2 / (2 * distance) + if deceleration > max_deceleration: + return True, max_deceleration + if deceleration < min_deceleration: + return False, 0.0 + + return True, deceleration + +def longitudinal_plan(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float, + method : str) -> 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. + """ + + if method == "milestone": + return longitudinal_plan_milestone(path, acceleration, deceleration, max_speed, current_speed) + elif method == "dt": + return longitudinal_plan_dt(path, acceleration, deceleration, max_speed, current_speed) + elif method == "dx": + return longitudinal_plan_dx(path, acceleration, deceleration, max_speed, current_speed) + else: + raise NotImplementedError("Invalid method, only milestone, dt, adn dx are implemented.") + + +def longitudinal_plan_milestone(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] + #============================================= + + print("-----LONGITUDINAL PLAN-----") + print("path length: ", path.length()) + length = path.length() + + # If the path is too short, just return the path for preventing sudden halt of simulation + if length < 0.05: + return Trajectory(path.frame, points, times) + + # This assumes that the time denomination cannot be changed + + # Starting point + x0 = points[0][0] + cur_point = points[0] + cur_time = times[0] + cur_index = 0 + + new_points = [] + new_times = [] + velocities = [] # for graphing and debugging purposes + + while current_speed > 0 or cur_index == 0: + # we want to iterate through all the points and add them + # to the new points. However, we also want to add "critical points" + # where we reach top speed, begin decelerating, and stop + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + print("=====================================") + # print("new points: ", new_points) + # print("current index: ", cur_index) + # print("current speed: ", current_speed) + # print("current position: ", cur_point) + + # Information we will need: + # Calculate how much time it would take to stop + # Calculate how much distance it would take to stop + min_delta_t_stop = current_speed/deceleration + min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 + # print(min_delta_x_stop) + assert min_delta_x_stop >= 0 + + + # Check if we are done + + # If we cannot stop before or stop exactly at the final position requested + if cur_point[0] + min_delta_x_stop >= points[-1][0]: + print("In case one") + # put on the breaks + + # Calculate the next point in a special manner because of too-little time to stop + if cur_index == len(points)-1: + # the next point in this instance would be when we stop + next_point = (cur_point[0] + min_delta_x_stop, 0) + else: + next_point = points[cur_index+1] + + # keep breaking until the next milestone in path + if next_point[0] <= points[-1][0]: + # print("continuing to next point") + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = next_point + current_speed -= deceleration*delta_t_to_next_x + cur_index += 1 + else: + # continue to the point in which we would stop (current_velocity = 0) + # update to the next point + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_point = next_point + cur_time += delta_t_to_next_x + # current_speed would not be exactly zero error would be less than 1e-4 but perfer to just set to zero + #current_speed -= delta_t_to_next_x*deceleration + current_speed = 0 + assert current_speed == 0 + + # This is the case where we are accelerating to max speed + # because the first if-statement covers for when we decelerating, + # the only time current_speed < max_speed is when we are accelerating + elif current_speed < max_speed: + # print("In case two") + # next point + next_point = points[cur_index+1] + # accelerate to max speed + + # calculate the time it would take to reach max speed + delta_t_to_max_speed = (max_speed - current_speed)/acceleration + # calculate the distance it would take to reach max speed + delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 + + delta_t_to_stop_from_max_speed = max_speed/deceleration + delta_x_to_stop_from_max_speed = max_speed*delta_t_to_stop_from_max_speed - 0.5*deceleration*delta_t_to_stop_from_max_speed**2 + + delta_t_to_next_point = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + velocity_at_next_point = current_speed + delta_t_to_next_point*acceleration + time_to_stop_from_next_point = velocity_at_next_point/deceleration + delta_x_to_stop_from_next_point = velocity_at_next_point*time_to_stop_from_next_point - 0.5*deceleration*time_to_stop_from_next_point**2 + # if we would reach max speed after the next point, + # just move to the next point and update the current speed and time + if next_point[0] + delta_x_to_stop_from_next_point < points[-1][0] and \ + cur_point[0] + delta_x_to_max_speed >= next_point[0]: + # ("go to next point") + # accelerate to max speed + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed += delta_t_to_next_x*acceleration + cur_index += 1 + + # This is the case where we would need to start breaking before reaching + # top speed and before the next point (i.e. triangle shape velocity) + elif cur_point[0] + delta_x_to_max_speed + delta_x_to_stop_from_max_speed >= points[-1][0]: + # print(delta_x_to_max_speed) + # print(delta_x_to_stop_from_max_speed) + # Add a new point at the point where we should start breaking + # print("Adding new point to start breaking") + delta_t_to_next_x = compute_time_triangle(cur_point[0], points[-1][0], current_speed, 0, acceleration, deceleration) + # print(delta_t_to_next_x) + #delta_t_to_next_x = compute_time_to_x(cur_point[0], points[-1][0] - min_delta_x_stop, current_speed, acceleration) + next_x = cur_point[0] + current_speed*delta_t_to_next_x + 0.5*acceleration*delta_t_to_next_x**2 + cur_time += delta_t_to_next_x + cur_point = [next_x, 0] + current_speed += delta_t_to_next_x*acceleration + + # this is the case where we would reach max speed before the next point + # we need to create a new point where we would reach max speed + else: + # print("adding new point") + # we would need to add a new point at max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + # This is the case where we are at max speed + # special functionality is that this block must + # add a point where we would need to start declerating to reach + # the final point + elif current_speed == max_speed: + next_point = points[cur_index+1] + # continue on with max speed + # print("In case three") + + # add point to start decelerating + if next_point[0] + min_delta_x_stop >= points[-1][0]: + # print("Adding new point to start decelerating") + cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed + cur_point = [points[-1][0] - min_delta_x_stop,0] + current_speed = max_speed + else: + # Continue on to next point + # print("Continuing on to next point") + cur_time += (next_point[0] - cur_point[0])/current_speed + cur_point = next_point + cur_index += 1 + + # This is an edge case and should only be reach + # if the initial speed is greater than the max speed + elif current_speed > max_speed: + # We need to hit the breaks + + next_point = points[cur_index+1] + # print("In case four") + # slow down to max speed + delta_t_to_max_speed = (current_speed - max_speed)/deceleration + delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 + + # If we would reach the next point before slowing down to max speed + # keep going until we reach the next point + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed -= delta_t_to_next_x*deceleration + cur_index += 1 + else: + # We would reach max speed before the next point + # we need to add a new point at the point where we + # would reach max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + else: + # not sure what falls here + raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") + + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + + points = new_points + times = new_times + print("[PLAN] Computed points:", points) + print("[TIME] Computed time:", times) + print("[Velocities] Computed velocities:", velocities) + + #============================================= + + trajectory = Trajectory(path.frame,points,times,velocities) + return trajectory + +def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: + """Computes the time to go from x0 to x1 with initial velocity v0 and final velocity v1 + with constant acceleration a. I am assuming that we will always have a solution by settings + discriminant equal to zero, i'm not sure if this is an issue.""" + + """Consider changing the system to use linear operators instead of explicitly calculating because of instances here""" + # x1 = x0 + v0*t + 0.5*a*t^2 + # x1 - x0 = v0*t + 0.5*a*t^2 + # 0.5*a*t^2 + v0*t + x0 - x1 = 0 + # t = (-v0 + sqrt(v0^2 - 4*0.5*a*(x0-x1)))/(2*0.5*a) + # t = (-v0 + sqrt(v0^2 + 2*a*(x1-x0)))/a + # print("x0: ", x0) + # print("x1: ", x1) + # print("v: ", v) + # print("a: ", a) + + t1 = (-v + max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a + t2 = (-v - max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a + + # print("t1: ", t1) + # print("t2: ", t2) + + if math.isnan(t1): t1 = 0 + if math.isnan(t2): t2 = 0 + + # print("t1: ", t1) + # print("t2: ", t2) + + valid_times = [n for n in [t1, t2] if n > 0] + if valid_times: + return min(valid_times) + else: + return 0.0 + +def compute_time_triangle(x0 : float, xf: float, v0: float, vf : float, acceleration : float, deceleration : float) -> float: + """ + Compute the time to go from current point assuming we are accelerating to the point at which + we would need to start breaking in order to reach the final point with velocity 0.""" + # xf = v0*t1 + .5a*t1^2 + v1t2 -0.5d*t2^2 + # 0 = v1 - d*t2 = v0 + a*t1 - d * t2 + # t1 = (d*t2 - v0)/a + # xf = v0*(d*t2 - v0)/a + 0.5*a*(d*t2 - v0)^2/a^2 + v1*t2 - 0.5*d*t2^2 + # xf = v0*d*t2/a - v0^2/a + 0.5*a*(d*t2^2 - 2*v0*d*t2 + v0^2)/a^2 + v1*t2 - 0.5*d*t2^2 + # 0 = t2^2(0.5*a*d/a^2 - 0.5*d) + t2(v0*d/a - v0*d*a/a^2 + v1) -v0^2/a +0.5*a*v0^2/a^2 -xf + roots = quad_root(0.5*acceleration + acceleration**2/deceleration - 0.5*acceleration**2/deceleration, + v0+2*acceleration*v0/deceleration - acceleration*v0/deceleration, + x0 - xf + v0**2/deceleration - 0.5*v0**2/deceleration) + print(roots) + t1 = max(roots) + assert t1 > 0 + return t1 + +def quad_root(a : float, b : float, c : float) -> float: + x1 = (-b + max(0,(b**2 - 4*a*c))**0.5)/(2*a) + x2 = (-b - max(0,(b**2 - 4*a*c))**0.5)/(2*a) + + # print("t1: ", t1) + # print("t2: ", t2) + + if math.isnan(x1): x1 = 0 + if math.isnan(x2): x2 = 0 + + # print("t1: ", t1) + # print("t2: ", t2) + + valid_roots = [n for n in [x1, x2] if not type(n) is complex] + # print(f"Valid roots {valid_roots}") + return valid_roots + +def solve_for_v_peak(v0: float, acceleration: float, deceleration: float, total_length: float) -> float: + + if acceleration <= 0 or deceleration <= 0: + raise ValueError("Acceleration and deceleration cant be negative") + + #Formuala: (v_peak^2 - v0^2)/(2*a) + v_peak^2/(2*d) = total_length + numerator = deceleration * v0**2 + 2 * acceleration * deceleration * total_length + denominator = acceleration + deceleration + v_peak_sq = numerator / denominator + + if v_peak_sq < 0: + return 0.0 + + return math.sqrt(v_peak_sq) + +def compute_dynamic_dt(acceleration, speed, k=0.005, a_min=0.5): + position_step = k * max(speed, 1.0) # Ensures position step is speed-dependent + return np.sqrt(2 * position_step / max(acceleration, a_min)) + +def longitudinal_plan_dt(path, acceleration: float, deceleration: float, max_speed: float, current_speed: float): + + + # 1 parametrizatiom. + path_norm = path.arc_length_parameterize(speed=1.0) + total_length = path.length() + + # ------------------- + # If the path is too short, just return the path for preventing sudden halt of simulation + if total_length < 0.05: + points = [p for p in path_norm.points] + times = [t for t in path_norm.times] + return Trajectory(path.frame, points, times) + # ------------------- + + # 2. Compute distances for d_accel,d_decel + if max_speed > current_speed: + d_accel = (max_speed**2 - current_speed**2) / (2 * acceleration) + else: + d_accel = 0.0 # Already at or above max_speed + + d_decel = (max_speed**2) / (2 * deceleration) + + # 3. trapezoidal or triangle? + if d_accel + d_decel <= total_length: + t_accel = (max_speed - current_speed) / acceleration if max_speed > current_speed else 0.0 + t_decel = max_speed / deceleration + d_cruise = total_length - d_accel - d_decel + t_cruise = d_cruise / max_speed if max_speed != 0 else 0.0 + t_final = t_accel + t_cruise + t_decel + profile_type = "trapezoidal" + else: + # Triangular profile: not enough distance to reach max_speed so we will calculate peak speed. + peak_speed = solve_for_v_peak(current_speed, acceleration, deceleration, total_length) + # choose the min just in case + peak_speed = min(peak_speed, max_speed) + t_accel = (peak_speed - current_speed) / acceleration if peak_speed > current_speed else 0.0 + t_decel = peak_speed / deceleration + t_final = t_accel + t_decel + profile_type = "triangular" + + t = 0 + times = [] + s_vals = [] + num_time_steps = 0 + while t < t_final: + times.append(t) + if profile_type == "trapezoidal": + if t < t_accel: + # Acceleration phase. + s = current_speed * t + 0.5 * acceleration * t**2 + elif t < t_accel + t_cruise: + # Cruise phase. + s = d_accel + max_speed * (t - t_accel) + else: + # Deceleration phase. + t_decel_phase = t - (t_accel + t_cruise) + s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase)**2 + else: # Triangular profile. + if t < t_accel: + # Acceleration phase. + s = current_speed * t + 0.5 * acceleration * t**2 + else: + t_decel_phase = t - t_accel + s_accel = current_speed * t_accel + 0.5 * acceleration * t_accel**2 + s = s_accel + peak_speed * t_decel_phase - 0.5 * deceleration * t_decel_phase**2 + + s_vals.append(min(s, total_length)) + if s >= total_length: + break + + dt = compute_dynamic_dt(acceleration if t < t_accel else deceleration,current_speed) + t = t + dt + + num_time_steps +=1 + + # Compute trajectory points + points = [path_norm.eval(s) for s in s_vals] + print("Number of time steps is --------------------", num_time_steps) + + # return Trajectory(path_norm.frame, points, times) + + + # # Plot: update a single window + # import matplotlib.pyplot as plt + # plt.figure("Distance vs Time") + # plt.clf() # Clear the current figure + # plt.plot(times, s_vals) + # plt.xlabel("Time (s)") + # plt.ylabel("Distance (m)") + # plt.title("Distance vs Time") + # plt.draw() + # plt.pause(0.001) + + + + # 4. Create a time grid. + # dt = 0.1 # adjust based on computation + # times = np.arange(0, t_final + dt, dt) + # num_time_steps = 0 + + # # 5. Compute the distance s(t) for each time step. + # s_vals = [] + # for t in times: + # if profile_type == "trapezoidal": + # if t < t_accel: + # # Acceleration phase. + # s = current_speed * t + 0.5 * acceleration * t**2 + # elif t < t_accel + t_cruise: + # # Cruise phase. + # s = d_accel + max_speed * (t - t_accel) + # else: + # # Deceleration phase. + # t_decel_phase = t - (t_accel + t_cruise) + # # Compute the remaining distance using the deceleration equation. + # s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase)**2 + # else: # Triangular profile. + # if t < t_accel: + # # Acceleration phase. + # s = current_speed * t + 0.5 * acceleration * t**2 + # else: + # t_decel_phase = t - t_accel + # s_accel = current_speed * t_accel + 0.5 * acceleration * t_accel**2 + # s = s_accel + peak_speed * t_decel_phase - 0.5 * deceleration * t_decel_phase**2 + # num_time_steps +=1 + + # # should not exceed total path length + # s_vals.append(min(s, total_length)) + # print("NUmber of time steps -----------",num_time_steps) + # print("T FInal ----------------------------", t_final) + # points = [path_norm.eval(s) for s in s_vals] + + + + trajectory = Trajectory(path_norm.frame, points, list(times)) + return trajectory + +def longitudinal_plan_dx(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] + #============================================= + # Adjust these two numbers to choose between computation speed or smoothness + rq = 0.1 # Smaller, smoother + multi = 5 # Larger, smoother + print("-----LONGITUDINAL PLAN-----") + print("path length: ", path.length()) + length = path.length() + + # If the path is too short, just return the path for preventing sudden halt of simulation + if length < 0.05: + return Trajectory(path.frame, points, times) + + # This assumes that the time denomination cannot be changed + + # Starting point + x0 = points[0][0] + cur_point = points[0] + cur_time = times[0] + cur_index = 0 + acc = 0 + + new_points = [] + new_times = [] + velocities = [] # for graphing and debugging purposes + + while current_speed > 0 or cur_index == 0: + # we want to iterate through all the points and add them + # to the new points. However, we also want to add "critical points" + # where we reach top speed, begin decelerating, and stop + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + print("=====================================") + print("new points: ", new_points) + print("current index: ", cur_index) + print("current speed: ", current_speed) + + # Information we will need: + # Calculate how much time it would take to stop + # Calculate how much distance it would take to stop + min_delta_t_stop = current_speed/deceleration + min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 + assert min_delta_x_stop >= 0 + + + # Check if we are done + + # If we cannot stop before or stop exactly at the final position requested + if cur_point[0] + min_delta_x_stop >= points[-1][0] - 0.0001: + acc = deceleration + flag = 1 + print("In case one") + # put on the breaks + # Calculate the next point in a special manner because of too-little time to stop + if cur_index >= len(points)-1: + # the next point in this instance would be when we stop + print(1) + if min_delta_x_stop < rq * acc: + next_point = (cur_point[0] + min_delta_x_stop, 0) + else: + next_point = (cur_point[0] + (min_delta_x_stop / (acc * multi)), 0) + flag = 0 + else: + print(2) + next_point = points[cur_index+1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + + # keep breaking until the next milestone in path + print("continuing to next point") + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = next_point + current_speed -= deceleration*delta_t_to_next_x + if flag: + cur_index += 1 + + # This is the case where we are accelerating to max speed + # because the first if-statement covers for when we decelerating, + # the only time current_speed < max_speed is when we are accelerating + elif current_speed < max_speed: + print("In case two") + print(current_speed) + acc = acceleration + flag = 1 + # next point + next_point = points[cur_index+1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + # accelerate to max speed + + # calculate the time it would take to reach max speed + delta_t_to_max_speed = (max_speed - current_speed)/acceleration + # calculate the distance it would take to reach max speed + delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 + + # if we would reach max speed after the next point, + # just move to the next point and update the current speed and time + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + print("go to next point") + # accelerate to max speed + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed += delta_t_to_next_x*acceleration + if flag: + cur_index += 1 + + # this is the case where we would reach max speed before the next point + # we need to create a new point where we would reach max speed + else: + print("adding new point") + # we would need to add a new point at max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + # This is the case where we are at max speed + # special functionality is that this block must + # add a point where we would need to start declerating to reach + # the final point + elif current_speed == max_speed: + next_point = points[cur_index+1] + # continue on with max speed + print("In case three") + + # add point to start decelerating + if next_point[0] + min_delta_x_stop >= points[-1][0]: + print("Adding new point to start decelerating") + cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed + cur_point = [points[-1][0] - min_delta_x_stop,0] + current_speed = max_speed + else: + # Continue on to next point + print("Continuing on to next point") + cur_time += (next_point[0] - cur_point[0])/current_speed + cur_point = next_point + cur_index += 1 + + # This is an edge case and should only be reach + # if the initial speed is greater than the max speed + elif current_speed > max_speed: + # We need to hit the breaks + acc = deceleration + flag = 1 + # next point + next_point = points[cur_index+1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + print("In case four") + # slow down to max speed + delta_t_to_max_speed = (current_speed - max_speed)/deceleration + delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 + + # If we would reach the next point before slowing down to max speed + # keep going until we reach the next point + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed -= delta_t_to_next_x*deceleration + cur_index += 1 + else: + # We would reach max speed before the next point + # we need to add a new point at the point where we + # would reach max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + else: + # not sure what falls here + raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") + + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + + points = new_points + times = new_times + print("[PLAN] Computed points:", points) + print("[TIME] Computed time:", times) + print("[Velocities] Computed velocities:", velocities) + + #============================================= + + 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] + + #============================================= + + print("=====LONGITUDINAL BRAKE=====") + print("path length: ", path.length()) + length = path.length() + + x0 = points[0][0] + t_stop = current_speed / deceleration + x_stop = x0 + current_speed * t_stop - 0.5 * deceleration * t_stop**2 + + new_points = [] + velocities = [] + + for t in times: + if t <= t_stop: + x = x0 + current_speed * t - 0.5 * deceleration * t**2 + else: + x = x_stop + new_points.append([x, 0]) + velocities.append(current_speed - deceleration * t) + points = new_points + print("[BRAKE] Computed points:", points) + + #============================================= + + trajectory = Trajectory(path.frame,points,times,velocities) + 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 + + self.min_deceleration = 1.0 + self.max_deceleration = 8.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): + start_time = time.time() + + 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 + + # Position in vehicle frame (Start (0,0) to (15,0)) + curr_x = vehicle.pose.x + curr_y = vehicle.pose.y + curr_v = vehicle.v + + abs_x = curr_x + state.start_vehicle_pose.x + abs_y = curr_y + state.start_vehicle_pose.y + + #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 + + lookahead_distance = max(10, curr_v**2 / (2 * self.deceleration)) + route_with_lookahead = route.trim(closest_parameter,closest_parameter + lookahead_distance) + print("Lookahead distance:", lookahead_distance) + + route_to_end = route.trim(closest_parameter, len(route.points) - 1) + + should_yield = False + yield_deceleration = 0.0 + + print("Current Speed: ", curr_v) + + for r in state.relations: + if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': + #get the object we are yielding to + obj = state.agents[r.obj2] + + detected, deceleration = detect_collision(abs_x, abs_y, curr_v, obj, self.min_deceleration, self.max_deceleration) + if isinstance(deceleration, list): + print("@@@@@ INPUT", deceleration) + time_collision = deceleration[1] + distance_collision = deceleration[0] + b = 3*time_collision - 2*curr_v + c = curr_v**2 - 3*distance_collision + desired_speed = (-b + (b**2 - 4*c)**0.5)/2 + deceleration = 1.5 + print("@@@@@ YIELDING", desired_speed) + route_with_lookahead = route.trim(closest_parameter,closest_parameter + distance_collision) + traj = longitudinal_plan(route_with_lookahead, self.acceleration, deceleration, desired_speed, curr_v) + return traj + else: + if detected and deceleration > 0: + yield_deceleration = max(deceleration, yield_deceleration) + should_yield = True + + print("should yield: ", should_yield) + + should_accelerate = (not should_yield 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, "milestone") + elif should_yield: + traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) + else: + traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v, "dt") + + return traj \ No newline at end of file From f3804dbc039b4a8674de8fb7101c6ec8f5c573a3 Mon Sep 17 00:00:00 2001 From: alo-20 Date: Mon, 17 Feb 2025 14:12:04 -0600 Subject: [PATCH 70/71] blink_component file test --- .../onboard/perception/state_estimation.py | 3 +- GEMstack/onboard/planning/blink_component.py | 33 ++++++---- .../planning/driving_logic_component.py | 42 ++++++++++++ homework/blink.py | 16 ++--- homework/blink_launch.yaml | 64 ------------------- launch/blink_launch.yaml | 6 +- 6 files changed, 75 insertions(+), 89 deletions(-) create mode 100644 GEMstack/onboard/planning/driving_logic_component.py delete mode 100644 homework/blink_launch.yaml diff --git a/GEMstack/onboard/perception/state_estimation.py b/GEMstack/onboard/perception/state_estimation.py index 4aef25659..db96ad38f 100644 --- a/GEMstack/onboard/perception/state_estimation.py +++ b/GEMstack/onboard/perception/state_estimation.py @@ -37,7 +37,8 @@ def state_outputs(self) -> List[str]: return ['vehicle'] def healthy(self): - return self.gnss_pose is not None + return True + # return self.gnss_pose is not None TODO: fix def update(self) -> VehicleState: if self.gnss_pose is None: diff --git a/GEMstack/onboard/planning/blink_component.py b/GEMstack/onboard/planning/blink_component.py index 9ee6a9164..acd40ed0e 100644 --- a/GEMstack/onboard/planning/blink_component.py +++ b/GEMstack/onboard/planning/blink_component.py @@ -1,6 +1,8 @@ from ..component import Component from ..interface.gem import GEMInterface,GEMVehicleCommand,GEMVehicleReading +from ...state import AllState,VehicleIntent import time +from typing import List TURN_OFF = 0 TURN_LEFT = 1 @@ -32,27 +34,30 @@ def cleanup(self): self.send_turn_command(TURN_OFF) # pass - def update(self): + def state_inputs(self) -> List[str]: + """Returns the list of AllState inputs this component requires.""" + return ["intent"] + + def update(self, state: AllState): """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. - - #if Allstate.intent.intent == "HALTING" : - #return current_time = time.time() - if current_time - self.last_update_time >= 2: # Change signal every 2 seconds - if self.turn_state == TURN_OFF: - self.turn_state = TURN_LEFT - elif self.turn_state == TURN_LEFT: - self.turn_state = TURN_RIGHT - else: - self.turn_state = TURN_OFF + if state.intent == 2: # check if halting + + if current_time - self.last_update_time >= 2: # Change signal every 2 seconds + if self.turn_state == TURN_OFF: + self.turn_state = TURN_LEFT + elif self.turn_state == TURN_LEFT: + self.turn_state = TURN_RIGHT + else: + self.turn_state = TURN_OFF - self.send_turn_command(self.turn_state) - self.last_update_time = current_time - + self.send_turn_command(self.turn_state) + self.last_update_time = current_time + # Read vehicle sensor data vehicle_reading = self.vehicle_interface.get_reading() print(f"Vehicle Speed: {vehicle_reading.speed:.2f} m/s") diff --git a/GEMstack/onboard/planning/driving_logic_component.py b/GEMstack/onboard/planning/driving_logic_component.py new file mode 100644 index 000000000..157fb90ad --- /dev/null +++ b/GEMstack/onboard/planning/driving_logic_component.py @@ -0,0 +1,42 @@ +from typing import List +from ..component import Component +from ...state import AllState,VehicleIntent + +class DrivingLogic(Component): + # count of times blinked + """Base class for top-level components in the execution stack.""" + def rate(self) -> float: + """Returns the rate in Hz at which this component should be updated.""" + return .5 + def state_inputs(self) -> List[str]: + """Returns the list of AllState inputs this component requires.""" + return ['all'] + def state_outputs(self) -> List[str]: + """Returns the list of AllState outputs this component generates.""" + return ['intent'] + def healthy(self): + """Returns True if the element is in a stable state.""" + return True + def initialize(self): + """Initialize the component. This is called once before the first + update.""" + self.count = 0 + return + def cleanup(self): + """Cleans up resources used by the component. This is called once after + the last update.""" + pass + def update(self, state: AllState): + """Update the component.""" + + state.intent.intent = 2 + + return + def debug(self, item, value): + """Debugs a streaming value within this component""" + if hasattr(self, 'debugger'): + self.debugger.debug(item, value) + def debug_event(self, label): + """Debugs an event within this component""" + if hasattr(self, 'debugger'): + self.debugger.debug_event(label) \ No newline at end of file diff --git a/homework/blink.py b/homework/blink.py index 89de04639..b83d3c49c 100644 --- a/homework/blink.py +++ b/homework/blink.py @@ -47,20 +47,20 @@ def cleanup(self): def get_dir_distress(self): pass - def update(self, Allstate): + 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. #self.turn_cmd = PacmodCmd() # TODO change to actual direction in Part 2 - if Allstate.intent.intent == "HALTING": - if self.turn_cmd.ui16_cmd == TURN_NONE: - self.turn_cmd.ui16_cmd = TURN_LEFT - elif self.turn_cmd.ui16_cmd == TURN_LEFT: - self.turn_cmd.ui16_cmd = TURN_RIGHT - else: - self.turn_cmd.ui16_cmd = TURN_NONE + + if self.turn_cmd.ui16_cmd == TURN_NONE: + self.turn_cmd.ui16_cmd = TURN_LEFT + elif self.turn_cmd.ui16_cmd == TURN_LEFT: + self.turn_cmd.ui16_cmd = TURN_RIGHT + else: + self.turn_cmd.ui16_cmd = TURN_NONE self.turn_blink_pub.publish(self.turn_cmd) pass diff --git a/homework/blink_launch.yaml b/homework/blink_launch.yaml deleted file mode 100644 index 0b17ac054..000000000 --- a/homework/blink_launch.yaml +++ /dev/null @@ -1,64 +0,0 @@ -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: - perception: - state_estimation : GNSSStateEstimator - perception_normalization : StandardPerceptionNormalizer - planning: - signaling: blink_component.BlinkDistress -# Driving behavior for the GEM vehicle. Runs perception and planner but doesn't execute anything (no controller). -drive: - perception: - state_estimation : GNSSStateEstimator - perception_normalization : StandardPerceptionNormalizer - planning: - signaling: 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/launch/blink_launch.yaml b/launch/blink_launch.yaml index 41174f692..f8b60eb2d 100644 --- a/launch/blink_launch.yaml +++ b/launch/blink_launch.yaml @@ -9,14 +9,16 @@ recovery: state_estimation : GNSSStateEstimator perception_normalization : StandardPerceptionNormalizer planning: - trajectory_tracking : blink_component.BlinkDistress + driving_logic: driving_logic_component.DrivingLogic + signaling: blink_component.BlinkDistress # Driving behavior for the GEM vehicle. Runs perception and planner but doesn't execute anything (no controller). drive: perception: state_estimation : GNSSStateEstimator perception_normalization : StandardPerceptionNormalizer planning: - trajectory_tracking : blink_component.BlinkDistress + driving_logic: driving_logic_component.DrivingLogic + signaling: blink_component.BlinkDistress log: # Specify the top-level folder to save the log files. Default is 'logs' folder : 'logs' From 466d887de7c0eab11a707874eed9cfb1cb994373 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Tue, 18 Feb 2025 18:33:33 -0600 Subject: [PATCH 71/71] Adjust Change dynamic_dt_k for simulation --- GEMstack/onboard/planning/longitudinal_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index bea4c6b68..d967d688e 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -444,7 +444,7 @@ def solve_for_v_peak(v0: float, acceleration: float, deceleration: float, total_ return math.sqrt(v_peak_sq) -def compute_dynamic_dt(acceleration, speed, k=0.005, a_min=0.5): +def compute_dynamic_dt(acceleration, speed, k=0.01, a_min=0.5): position_step = k * max(speed, 1.0) # Ensures position step is speed-dependent return np.sqrt(2 * position_step / max(acceleration, a_min))