Add localization with MCL and semantic segmentation #326
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
added 30 commits
January 26, 2023 13:41
…or into feature_mcl_with_semseg
|
@danielv012 FYI, added some untracked documentation |
wheitman
pushed a commit
that referenced
this pull request
Mar 29, 2023
* Fix URDF mesh, ports, and lidar - URDF mesh now points to /home/share on Quad. 3D model works in Rviz2 - Port for CARLA now points to 2000+domain id - Domain ID of 14 => -world-port=2014 - Domain ID now passed to Docker from host on boot - Fixed error with lidar processing node - Removed one last reference to AllStar - Deleted stale 3D models from /data :sparkles: Add Guardian node :sparkles: Add working CARLA simulation agent (`carla.launch.py`) :sparkles: Add junction management node (basic FSM) :sparkles: Add grid summation node :sparkles: Add RTP, the recursive tree planner, with inbuilt motion control :sparkles: Add airbags, a low-level emergency braking system Numerous other refinements and integrations in preparation for real-world use! See below --- * Copy ground seg code over from Stepan * Comment out log message * Fix package boilerplate * fixed/modified subsriber, publisher for dynamic_grid * occupancy grid appears in rviz2 * Update index.md * Removed old EvidentialGrid msgs, OccupancyGrid now in use for Egma * - Fixed egma.msg - Added ROS_DOMAIN_ID to carla bridge port * Removed refrence to nonfunctional rrt node, fixed error in lite launch * Updated reference to cycloneDDS in dockerfile * Add MCL, mapping tools, and leaderboard connection improvments (#303) * Initial commit * Fix MCL import, PCD read in mcl_node * Add octomap package * Add PCL conversion * Add tree.insertPointCloud * Mapping works * Edit Docker files, gitignore * Add Rviz2 to launch * Source Navigator. Does not appear to work 100%... * It crashes immediately 🥲 * Add repub to world info in map_management_node * Begin mapping doc * Fixed runtime error * Add to mapping doc * Add cube list visuals for voxel map * Switch to PointCloud2 for visuals. - Much faster. - Can still be shown as voxels ("boxes") in Rviz2 * Rename to OctoSlam, add MCL boilerplate * Add particle visualization (kinda) * Fix intial covariance, add more boilerplate * General updates to particle filter * Add diagnostic pub * Move tf broadcasting to OctoSlam * Revise predictParticleMotion * Adjust height cutoff function in lidar_processing * Escape pointer hell * Ignore map files * Separate weights into another vector * Add (dubious) performance timers * Move quatToYaw to Eigen. It ((sorta)) works * Plot lane boundaries in map_management * Add opendrivepy * Add road parsing * Add to opendrivepy * Contains kinda works, shows image * Fix overlap in junctions, add tqdm * Map grid now shown in Rviz * Fix occupancy grid origin * Add Roger Labbe's code * Add to mcl.py * Add semantic lidar (road only) * Add to MCL.step() * Transforms published * Add particle vis * Write alignment for update_weights * Continuing to tune MCL * All green lights * Finished with MCL for now :( * Fix yaw issue with MCL * Refactor topic names and MCL code * Fix MCL init error to far from GNSS * Dust off the Leaderboard connection workflow * Move to Town12 * Add libopendrive, map_management_cpp * Finish get_lane_polygons. - Plotting works. - Still need to generate occupancy grid... * Improve efficiency of getOccupancyGrid * MapManager now publishes drivable grid * Add to map manager * Map now loads from /carla/world_info * Refactor, fix leaderboard * Rename voltron_msgs to nova_msgs * Rename `map_management_cpp` to `map_management` - Fix build tool error with libopendrive * Add segmentation pkg * Add mmseg to /mmsegmentation - Install with Dockerfile * Add mmseg to image_segmentation_node (again) * Update main.yml Edit main.yaml to build Navigator using latest Docker image * Update action to use checkout#v3 * Complete 2D image segmentation * Add ROSOccupancyGridPrediction - Also remove dynamic_grid_old * Create occupancy_cpp package * Add to GroundSegmentationNode * Fixed "PCL can not be found" with symlink * Add to docs * Added MRF ground filtering * Add StaticOccupancyNode * Add ground seg to carla.launch.py * Add to getLanesFromRouteMsg * Fix path publication * add points to DST logic implemented * Cleaned up * Completed add free spaces and add ego vehicle logic. Cleaned up as well. Still needs some adustment * Added header file, nova_msgs build error * Add to route manipulation * Debugging, symbol lookup error * Comment out "masses" references * Fix map_manager, remove prob_0 * Finish removal of prob_0 * Remove occupancy_msg * Add debug to add_points_to_the_DST * Remove PI, got "did not match a case" * Add routing to MapManager * Finish routing for now - It's jank - We're going to move on anyway 👍 * Rename MapManager members * Add simple_route_controller * Fixed slope error, now publishes. Grid values incorrect. * Refactored names. Easier to read. * Add route distance costmap * Route distance now continuous - Distance previous was relative to closest route point, which created a series of circles - Distance now relative to linestring, creating a tube shape * Changed doubles to floats. Grid works! I think it's rotated incorrectly though * Add to docs * Add to docs * Add 3D semantic segmentation (#320) * Begin adapatation of BerensRWU/DenseMap * LiDAR->image projection kinda works * Add documentation, wrap up * Add to launch * Add NPC car spawning * Add walker spawn * Got rid of more doubles, changed subscribed topic to ground seg node. Still flipped and rotated incorrectly?? * Begin to switch mcl to base_link grid * Add flat surface grid * Steady progress. Need to adjust dims of drivable area grid. * Adjust map publisher dimensions * Still not quite right. * Adjusting for loop iteration to see if that helps * Converted tesor and mass callback function * Keeping this configuration for now, still rotated * Begin move to speedometer for motion prediction * Finish addition of speedometer data * Re-add weights, but car is jumpy in motion * Hits calculation now fixed * Fixed an oopsie with getMotionDelta * Still jank :`( * Add kidnap warning, noise * Incomplete switch to voting method * Switched rows and columns, sort of working now * Testing stage for prednet node * Now ignores points outside of boundary. Near perfect, H/V raytracing needs work * Logic is done! I concede to minor anomalies. Refactoring and masses TODO * Updated installation instructions * Speed up semantic segmentation * Semantic LiDAR is now colorized * Masses time * Re-add particle-on-grid transforms * Back to square one. Kinda works, not really * Add pole penalty * Add highly accurate dead reckoning * Add "tricks" page * Switch to "semantic" grid map * Add map object tree * Add signals * Add getLandmarks * Get nearest landmark * Re-add original weight update * Static Grid Exe now runs through main stack * Fixed nav_msgs undefined symbol error. Masses now working. * created epas node * Refactored and documented code. Still needs more refactoring but ready for use. * Add alternate logo * Battling memory issues in MapManagementNode * added can communication logic to epas node * Adjust Dockerfile * Pulled from odev * Add to mcl.py * Done for now * Begin to re-introduce traffic light points * Fix StaticOccupancy build error, remove LGSVL msgs * Final touches * Add GNSS averaging node * Add subscriptions * Changed constant names, builds and runs correctly. * Add localization with MCL and semantic segmentation (#326) * Begin move to speedometer for motion prediction * Finish addition of speedometer data * Re-add weights, but car is jumpy in motion * Hits calculation now fixed * Fixed an oopsie with getMotionDelta * Still jank :`( * Add kidnap warning, noise * Incomplete switch to voting method * Speed up semantic segmentation * Semantic LiDAR is now colorized * Re-add particle-on-grid transforms * Back to square one. Kinda works, not really * Add pole penalty * Add highly accurate dead reckoning * Add "tricks" page * Switch to "semantic" grid map * Add map object tree * Add signals * Add getLandmarks * Get nearest landmark * Re-add original weight update * Add alternate logo * Battling memory issues in MapManagementNode * Add to mcl.py * Done for now * Begin to re-introduce traffic light points * Fix StaticOccupancy build error, remove LGSVL msgs * Final touches * Add documentation and RTPS files * Add untracked documentation * Resolve merge conflict * Upaded package json * Moved prednet models to data directory, fixed reference in node file * finished coding epas_node * Remove reference to lanelet * add throttle_node * Initial commit for parade controller * Add GNSS averaging node - Error is between 0.7-2.0 meters - Likely projection error b/w CARLA, pymap3d * Updated UTM projection function * Fix memory leak, add docs to image_segmentation * Add diagnostics to gnss averager * Starting searching for predent import fix * Fix most build errors - StaticOccupancy variable mismatch - X forwarding - Lanelet missing * Finished prednet node * Rotated Grid & Changed to Egma message * created, built and ran joy node * created subsriber for epas_node * Finished comments on prednet node * Moved packages to parade launch file, added joy_linux driver * Add linear actuator node * Removed carla packages, changed port to ACM1 * Parade controller package creation * Changed param files to left and right, TODO: add remaining driver/scan packages * Add throttle control node * Update EPAS and MCU interface packages * Changed file header * Add grid summation/cost map generation (#337) * created epas node * added can communication logic to epas node * Adjust Dockerfile * finished coding epas_node * Remove reference to lanelet * add throttle_node * Fix most build errors - StaticOccupancy variable mismatch - X forwarding - Lanelet missing * created, built and ran joy node * created subsriber for epas_node * Moved packages to parade launch file, added joy_linux driver * Add linear actuator node * Removed carla packages, changed port to ACM1 * Parade controller package creation * Changed param files to left and right, TODO: add remaining driver/scan packages * Add throttle control node * Update EPAS and MCU interface packages * Add grid summation node * Add to grid summation, docker.sh * Fix launch file, move parade controller * Add to grid summation * Add grid summation to leaderboard workflow --------- Co-authored-by: ashwinsom <ashwin.som2001@gmail.com> Co-authored-by: jai peris <jperis390@gmail.com> Co-authored-by: danielv012 <daniel@vayman.co> * Add RRT goal pose: `/planning/goal_pose` (#338) --------- Co-authored-by: ashwinsom <ashwin.som2001@gmail.com> Co-authored-by: jai peris <jperis390@gmail.com> Co-authored-by: danielv012 <daniel@vayman.co> * Switch to EGMA for cost map (#339) * Replaced all references to lidar_front/rear to right/left. Parade launch file now contains necessary lidar packages * subs n pubs * Filters pointcloud and finds banner x, y * Create airbag package * Finish airbags (for now) * Feature airbags (#340) * Create airbag package * Finish airbags (for now) * Add to rtp * Double res of route distance * Basic logic done. TODO: pcl concatenation, I & D terms for PID * Ready for testing * Fix right lidar tf * Parade controller tuning * Get MCU * Lidar_front -> Lidar_right transform * Experiment with Town02 routing * Added ROS_DOMAIN_ID to leaderboard script * Add to routes.md * Add to route generation refinements * Add Town02 routes for lb testing * Add rough section * Add BFS for routing (route not yet generated) * Can now print route from BFS * Begin route reorientation/correction * Route works well enough for now. - Still crashes sometimes... * Fix route near/edge indices calculation * Downsample grids for 4x speedup * Continue routing refinements * Re-add map_manager to launch * Route distance works * Goal pose publisher re-added * Finish recurvise tree planner * Remove costmap debug file * Add commands to RTP * Epas, Joy, Linear Actuator ✅ * Forgot this guy * Route completion now at 31% * Add airbags (kinda) * Fixed yaw calculation in map_management * Further refine RTP * Add airbag marker * Add predicted occupancy * Add junction cost * Simplify airbags * pulling form dev * Fixed prediction framerate * Add NMEA navsat driver * Speed up map management grid pub * Add getBarrierIndex to rtp * Add barrier marker * Add barrier braking * Refine barrier logic * Add ROS web bridge * Add barrier penalty * Boost RTP resolution at root step * Fix eroson for ego_in_junction * Publish route progress * Purge old files * Rename "grids" to "costs" * Add junction manager * Begin joy_translation refactor * Add center camera * Add MJPEG camera streaming * Add Guardian node * Firmware changes * Forsook the mode request service - Replaced with a lame publisher - Service call hung/froze/blocked * Updated EPAS & MCU code * Updated parade launch file, now has lidar drivers * Add manual control * Add to Guardian system * Add camera package * Add to parade.launch.py * Move "parade" to "main.launch.py" in root * Add real-world info to docs --------- Co-authored-by: ashwinsom <ashwin.som2001@gmail.com> Co-authored-by: Daniel Vayman <daniel@vayman.co> Co-authored-by: danielv012 <daniel@vayman.com> Co-authored-by: Jesse Musa <jessemusa2@gmail.com> Co-authored-by: jai peris <jperis390@gmail.com>
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Add localization with MCL and semantic segmentation
See
state_estimationpackage.mcl.mp4