Skip to content
This repository has been archived by the owner on Apr 24, 2023. It is now read-only.

Feat/slam gtsam #829

Merged
merged 47 commits into from
Jun 5, 2021
Merged

Feat/slam gtsam #829

merged 47 commits into from
Jun 5, 2021

Conversation

VAM7686
Copy link
Contributor

@VAM7686 VAM7686 commented Jun 4, 2021 •
edited
Loading

Description

Uses the GTSAM library to implement 3D localization using a factor graph. GPS coordinates, IMU measurements, magnetometer measurements, and wheel odometry measurements are continually integrated into the factor graph which helps localizes the robot over time. With 3D localization we are able to go over uneven terrain such as ramps.

This PR does the following:
Main focus:

  • Implements 3D localization using a factor graph.

Other fixes:

  • Fixes GPS plugin configuration issues
  • Added magnetometer integration in robot_localization ekf_localization_node configuration

Fixes #555 and #662

Testing steps (If relevant)

Test Case 1

  1. Launch autonav.launch
  2. Launch the 3D localization and navigation using the 3D_nav_simulation.launch
  3. Launch rviz and display ground truth from the topic /ground_truth and display the slam estimate from the topic /odometry/filtered

Expectations:

  1. slam estimate matches the ground truth.
  2. robot is able to go over ramps.

Self Checklist

  • I have formatted my code using make format
  • I have tested that the new behavior works

andrewyarovoi and others added 30 commits November 24, 2019 23:01
@andrewyarovoi
Incorporated odometry measurements to the factor graph
6070745
@andrewyarovoi
Added support for imu but it is not fully working yet.
3a3c7a7
@andrewyarovoi
fixed small issue with symbols
c077556
@andrewyarovoi
Fixed missing boost library issue
088d2bd
@andrewyarovoi
Finally got it to compile again
41c4ad5
@andrewyarovoi
Saving changes before reinstalling ubuntu
c7d5d64
@andrewyarovoi
changed CMakeList.txt to closer reflect Jason's file
a726570
@andrewyarovoi
working imu and odom factors
bb3ca97
@andrewyarovoi
added GPS factors
68caa59
@andrewyarovoi
Merge branch 'master' of https://github.com/RoboJackets/igvc-software …
32fad41 
…into feat/slam-gtsam
@andrewyarovoi
checked for covariance equaling zero
588fae3
@andrewyarovoi
Merge branch 'master' of https://github.com/RoboJackets/igvc-software …
bb3437c 
…into feat/slam-gtsam
@andrewyarovoi
changed the values of wheel spacing
750170d
@andrewyarovoi
Merge branch 'master' of https://github.com/RoboJackets/igvc-software …
2d73c2a 
…into feat/slam-gtsam
@andrewyarovoi
removed faulty odometry and just using GPS and IMU
298156d
@andrewyarovoi
merged in master branch
3420a16
@andrewyarovoi
added magnetometer to gazebo simulation
5d635ea
@andrewyarovoi
created a conversion node so that gazebo publishes correct message type
416af9a
@andrewyarovoi
working on adding mag factor
72163da
@andrewyarovoi
Merge branch 'master' of https://github.com/RoboJackets/igvc-software …
d0261e1 
…into feat/slam-gtsam
@andrewyarovoi
added custom mag factor
ed21025
@andrewyarovoi
make format
fecb424
@oswinso
Fixes for MagPoseFactor jacobian bug
a494974
@andrewyarovoi
Added debugging mode
50e7fb1
@andrewyarovoi
working 3D localization, but teb is slow
a49dcc4
@andrewyarovoi
added twist to odom messages
3224a2b
@andrewyarovoi
Change a param in igvc_control.launch
5780b8b 
The wheel radius was accidentally set to the wheel diameter causing the robot to move slowly. This commit will change the param to its correct value.
@andrewyarovoi
change linear velocity to be in the local frame
746e8aa
@andrewyarovoi
Fix issue that caused the robot to move extremely slowly in the y dir…
91c0ffe 
…ection
@matthewhannay567
Updated submodules
fc9bc6c
matthewhannay567 and others added 11 commits February 14, 2021 15:51
@matthewhannay567
Removed 'rosdep update' from install_dependcies
cc48622
@matthewhannay567
make install_dependencies ignore "serial" dep
2e0b48c
@matthewhannay567
Fixed .gitmodules
69cfa64
@VAM7686
Merge branch 'master' into feat/slam-gtsam
a17ff50
@VAM7686
Changed frame_ids so can visualize odom in rviz. Added magnetometer r…
0a598ba 
…eadings into ekf_localization_node_params so initial orientation accounted for in robot_localization pose_ekf
@VAM7686
ekf works correctly now
f44172c
@VAM7686
fixed navsat problem but I have no clue why this works
c23efe0
@VAM7686
Removed navsat_transform_node stuff and integrated raw gps and wheel …
cfa49d1 
…odom measurements directly. Weird stuff happening with gps still.
@VAM7686
slam finally working!
feee90b 
Realized that there was a problem with the hector gazebo plugin which was producing the wrong latitudes and longitudes in the simulation. The wrong behavior was that along the +X axis (East) latitude increased and along the +Y axis (North) longitude decreased. Instead we needed for +X to increase longitude and +Y to increase latitude. Thus I had to set the reference_heading in the jessii.urdf.xacro file to be 90 to fix this issue. Weird how its been going on for so long. Surprised nobody caught this for so long.::
@VAM7686
formatted using make format
d5d2850
@VAM7686
removed navsat transform stuff from launch
22959d3
@matthewhannay567 matthewhannay567 changed the base branch from master to jessiii-master June 5, 2021 15:53
@matthewhannay567 matthewhannay567 merged commit a353a47 into jessiii-master Jun 5, 2021
@matthewhannay567 matthewhannay567 deleted the feat/slam-gtsam branch June 5, 2021 19:52
@matthewhannay567 matthewhannay567 restored the feat/slam-gtsam branch June 5, 2021 19:53
Sign up for free to subscribe to this conversation on GitHub. Already have an account? Sign in.
Reviewers

@matthewhannay567 matthewhannay567 matthewhannay567 approved these changes

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.

Integrate SLAM into navigation stack
4 participants
@VAM7686 @matthewhannay567 @andrewyarovoi @oswinso