Skip to content

Commit

Permalink
Successfully fused encoder and imu data with ekf for robot_localization
Browse files Browse the repository at this point in the history
  • Loading branch information
TheNoobInventor committed Oct 11, 2023
1 parent ae83584 commit e83c949
Show file tree
Hide file tree
Showing 6 changed files with 60 additions and 72 deletions.
4 changes: 3 additions & 1 deletion .gitignore
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,4 @@
.vscode/
.vale.ini
.vale.ini
lidarbot_base/src/motor_node.cpp
lidarbot_base/src/motor_diagnostics_node.cpp
4 changes: 3 additions & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ The following components were used in this project:

| | Part |
| --| --|
|1| [Raspberry Pi 4 (4 GB)](https://www.raspberrypi.com/products/raspberry-pi-4-model-b/)|
|1| Raspberry Pi 4 (2 GB)|
|2| SanDisk 32 GB SD Card (minimum)|
|3| [Two wheel drive robot chassis kit (with wheel encoders)](https://www.amazon.com/perseids-Chassis-Encoder-Wheels-Battery/dp/B07DNYQ3PX/ref=sr_1_9?crid=3T8FVRRMPFCIX&keywords=two+wheeled+drive+robot+chassis&qid=1674141374&sprefix=two+wheeled+drive+robot+chas%2Caps%2C397&sr=8-9)|
|4| [Waveshare Motor Driver HAT](https://www.waveshare.com/wiki/Motor_Driver_HAT)|
Expand Down Expand Up @@ -643,6 +643,8 @@ Drive around the environment to generate a map:

Then save the generated map.

TODO: Need to change slam_toolbox ROS parameter mode to `mapping` in `mapper_params_online_async.yaml` in `lidarbot_slam/config`. Show a snippet of the code and what needs to be changed for mapping. Same thing applies for navigation/localization

## Navigation

### Gazebo
Expand Down
34 changes: 15 additions & 19 deletions lidarbot_bringup/config/controllers.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ controller_manager: # Node name
diff_controller:
ros__parameters:

publish_rate: 50.0 #60.0 # Has to be a double otherwise errors are encountered
publish_rate: 50.0 # Has to be a double otherwise errors are encountered

base_frame_id: base_footprint

Expand All @@ -24,32 +24,28 @@ diff_controller:
wheel_separation: 0.134
wheel_radius: 0.0335

pose_covariance_diagonal: [0.05, 0.05, 0.0, 0.0, 0.0, 0.01]
twist_covariance_diagonal: [0.05, 0.05, 0.0, 0.0, 0.0, 0.01]

# pose_covariance_diagonal: [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
# twist_covariance_diagonal: [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]

# pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.01]
# twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.01]

# pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
# twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
# Obtained from https://wiki.ros.org/diff_drive_controller
pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]

# Whether the input has a time stamp or not
use_stamped_vel: false

# Odometry fused with IMU is published by robot_localization, so no need to publish a TF
# based on encoders alone (https://wiki.ros.org/diff_drive_controller)
enable_odom_tf: false

imu_broadcaster:
ros__parameters:

sensor_name: mpu6050
frame_id: imu_link

static_covariance_orientation: [0.005, 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.0, 0.005] # Imu orientation not fused so this might not matter
static_covariance_angular_velocity: [5.76e-04, 0.0, 0.0, 0.0, 5.76e-04, 0.0, 0.0, 0.0, 5.76e-04]
static_covariance_linear_acceleration: [9.8067e-03, 0.0, 0.0, 0.0, 9.8067e-03, 0.0, 0.0, 0.0, 9.8067e-03]
# static_covariance_orientation: [0.005, 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.0, 0.005]
# static_covariance_angular_velocity: [5.76e-04, 0.0, 0.0, 0.0, 5.76e-04, 0.0, 0.0, 0.0, 5.76e-04]
# static_covariance_linear_acceleration: [9.8067e-03, 0.0, 0.0, 0.0, 9.8067e-03, 0.0, 0.0, 0.0, 9.8067e-03]

# static_covariance_orientation: [0.005, 0.0, 0.0, 0.005, 0.0, 0.0, 0.005, 0.0, 0.0] # Imu orientation not fused so this might not matter
# static_covariance_angular_velocity: [5.76e-04, 0.0, 0.0, 5.76e-04, 0.0, 0.0, 5.76e-04, 0.0, 0.0]
# # static_covariance_linear_acceleration: [9.8067e-03, 0.0, 0.0, 9.8067e-03, 0.0, 0.0, 0.0, 0.0, 0.0]
# static_covariance_linear_acceleration: [9.8067e-03, 0.0, 0.0, 9.8067e-03, 0.0, 0.0, 9.8067e-03, 0.0, 0.0]
# 500 data points
static_covariance_orientation: [2.63882e-06, 0.0, 0.0, 0.0, 7.50018e-06, 0.0, 0.0, 0.0, 2.89257e-09]
static_covariance_angular_velocity: [2.71413e-07, 0.0, 0.0, 0.0, 6.79488e-07, 0.0, 0.0, 0.0, 4.37879e-07]
static_covariance_linear_acceleration: [0.00133755, 0.0, 0.0, 0.0, 0.000209753, 0.0, 0.0, 0.0, 0.00143276]
2 changes: 1 addition & 1 deletion lidarbot_description/urdf/gazebo_control.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_footprint</robot_base_frame>
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_odom_tf>false</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>

</plugin>
Expand Down
2 changes: 1 addition & 1 deletion lidarbot_description/urdf/imu.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@
<sensor name="mpu6050_imu" type="imu">
<plugin name="imu_controller" filename="libgazebo_ros_imu_sensor.so">
<ros>
<remapping>~/out:=imu_broadcaster/data</remapping>
<remapping>~/out:=imu_broadcaster/imu</remapping>
</ros>
<initial_orientation_as_reference>false</initial_orientation_as_reference>
</plugin>
Expand Down
86 changes: 37 additions & 49 deletions lidarbot_navigation/config/ekf.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ ekf_filter_node:

use_sim_time: false

frequency: 50.0 #80.0
frequency: 50.0
two_d_mode: true
print_diagnostics: true

Expand All @@ -14,15 +14,8 @@ ekf_filter_node:
world_frame: odom

odom0: diff_controller/odom
odom0_differential: false
odom0_relative: true
# odom0_queue_size: 40
imu0: imu_broadcaster/imu

imu0: imu_broadcaster/data
imu0_differential: false
imu0_relative: true
# imu0_queue_size: 50

# [x_pos , y_pos , z_pos,
# roll , pitch , yaw,
# x_vel , y_vel , z_vel,
Expand All @@ -31,59 +24,54 @@ ekf_filter_node:

odom0_config: [false, false, false,
false, false, false,
true, true, false,
true, true, false,
false, false, true,
false, false, false]

imu0_config: [false, false, false,
false, false, true,
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
true, false, false]

# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
# However, if users find that a given variable is slow to converge, one approach is to increase the
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
# unspecified.
process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.05 #x
0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.05 #y
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.06 #z
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.03 #roll
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.03 #pitch
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.06 #yaw
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.025 #vx
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.025 #vy
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.04 #vz
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.01 #vroll
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.01 #vpitch
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, #0.02 #vyaw
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, #0.01 #ax
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.01 #ay
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #0.015 #az
# process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.05 #x
# 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.05 #y
# 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.06 #z
# 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.03 #roll
# 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.03 #pitch
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.06 #yaw
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.025 #vx
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.025 #vy
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #0.04 #vz
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, #0.01 #vroll
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, #0.01 #vpitch
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, #0.02 #vyaw
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, #0.01 #ax
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, #0.01 #ay
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] #0.015 #az


# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
# question. Users should take care not to use large values for variables that will not be measured directly. The values
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
#if unspecified.
initial_estimate_covariance: [1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #x
0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #y
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #z
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #roll
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #pitch
0.0, 0.0, 0.0, 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #yaw
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #vx
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #vy
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #vz
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #vroll
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #vpitch
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, #1e-9 #vyaw
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-3, 0.0, 0.0, #1e-9 #ax
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #ay
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #1e-9 #az

# TODO:
# Made vy component of process_noise_covariance and initial_estimate_covariance 0.0
# initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #x
# 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #y
# 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #z
# 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #roll
# 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #pitch
# 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #yaw
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #vx
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #vy
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #vz
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, #1e-9 #vroll
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, #1e-9 #vpitch
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, #1e-9 #vyaw
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, #1e-9 #ax
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, #1e-9 #ay
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] #1e-9 #az

0 comments on commit e83c949

Please sign in to comment.