Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Ekf localization #127

Merged
merged 18 commits into from
Jul 7, 2023
Merged
Show file tree
Hide file tree
Changes from 11 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
63 changes: 63 additions & 0 deletions panther_bringup/config/ekf_config.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,63 @@
# Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html

frequency: 20
rafal-gorecki marked this conversation as resolved.
Show resolved Hide resolved
sensor_timeout: 0.2
two_d_mode: true

transform_time_offset: 0.0
transform_timeout: 0.1

map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
publish_tf: true
publish_acceleration: false

odom0: odom/wheels
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: true

imu0: imu/data
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
true, true, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5

imu0_remove_gravitational_acceleration: true

reset_on_time_jump: false
predict_to_current_time: false
print_diagnostics: false
debug: false
debug_out_file: /path/to/debug/file.txt

# Selected values ​​experimentally so as to ensure relatively fast convergence (values ​​should be about 10x higher than the sensor variacne values)
rafal-gorecki marked this conversation as resolved.
Show resolved Hide resolved
dynamic_process_noise_covariance: true
process_noise_covariance: [1e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 3e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 3e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 3e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 2e-5, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 2e-5, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 2e-5, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-5, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-5, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-5, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-5, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-5, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-5]
15 changes: 11 additions & 4 deletions panther_bringup/config/imu_config.yaml
Original file line number Diff line number Diff line change
@@ -1,19 +1,26 @@
ImuFilterNodelet:
gain: 0.1
zeta: 0.0
mag_bias_x: 0.0
mag_bias_y: 0.0
mag_bias_z: 0.0
orientation_stddev: 1e-2
publish_tf: false
use_mag: false
use_magnetic_field_msg: false
use_magnetic_field_msg: true # If set to true, subscribe to the /imu/mag topic as a sensor_msgs/MagneticField; if set to false (deprecated), use geometry_msgs/Vector3Stamped.
rafal-gorecki marked this conversation as resolved.
Show resolved Hide resolved
fixed_frame: imu_link
stateless: false
remove_gravity_vector: false

PhidgetsSpatialNodelet:
data_interval_ms: 8 # supported data rates: 4 8 16 24 32 40 ... 1000 (in ms)
publish_rate: 100.0 # optional param publish_rate, defaults to 0
# serial: 123456 # optional param serial, default is -1
# hub_port: 0 # optional param hub_port, used if connected to a VINT hub
# frame_id: imu_link # optional param frame_id, default is "imu_link"
# linear_acceleration_stdev: 0.002745862 # optional param linear_acceleration_stdev, default is 280ug
# angular_velocity_stdev: 0.000349 # optional param angular_velocity_stdev, default is 0.095 deg/s
# magnetic_field_stdev: 1.1e-7 # optional param magnetic_field_stdev, default is 1.1 milligauss
linear_acceleration_stdev: 3.5e-3 # Currently used IMU specification 10 * (2mg ± 1.5mg) (Accelerometer Drift Max ± Accelerometer Noise for 100 Hz) (https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1205)
angular_velocity_stdev: 1.5e-2 # Currently used IMU specification 0.1°/s ± 0.05°/s (Gyroscope Drift Max ± Gyroscope Noise for 100 Hz) (https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1205)
# magnetic_field_stdev: 10.0 # Currently used IMU specification ± 10mG (± Magnetometer Noise) (https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1205)
rafal-gorecki marked this conversation as resolved.
Show resolved Hide resolved

# compass correction params (see http://www.phidgets.com/docs/1044_User_Guide)
# cc_mag_field: 0.52859
Expand Down
2 changes: 1 addition & 1 deletion panther_bringup/config/panther_common.yaml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
can_interface: panther_can
odom_frame: odom
base_link_frame: base_link
publish_tf: true
publish_tf: false
publish_odom: true
publish_pose: false
publish_joints: true
Expand Down
2 changes: 2 additions & 0 deletions panther_bringup/launch/bringup.launch
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,8 @@

<include file="$(find panther_bringup)/launch/imu.launch" />

<include file="$(find panther_bringup)/launch/ekf.launch" />

</group>

<include file="$(find panther_description)/launch/panther_state_publisher.launch" if="$(arg publish_robot_state)">
Expand Down
7 changes: 7 additions & 0 deletions panther_bringup/launch/ekf.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
<launch>

<node pkg="robot_localization" type="ekf_localization_node" name="panther_ekf" clear_params="true">
<rosparam command="load" file="$(find panther_bringup)/config/ekf_config.yaml" />
</node>

</launch>
1 change: 1 addition & 0 deletions panther_bringup/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -24,5 +24,6 @@
<depend>panther_msgs</depend>
<depend>panther_power_control</depend>
<depend>phidgets_spatial</depend>
<depend>robot_localization</depend>

</package>
4 changes: 4 additions & 0 deletions panther_description/config/WH01.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -6,3 +6,7 @@ inertia_y_offset: 0.0
mesh_package: panther_description
folder_path: meshes/WH01
kinematics: differential

velocity_x_stderr: 3.2e-3
rafal-gorecki marked this conversation as resolved.
Show resolved Hide resolved
velocity_y_stderr: 1.4e-3
velocity_yaw_stderr: 8.5e-3
4 changes: 4 additions & 0 deletions panther_description/config/WH02.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -6,3 +6,7 @@ inertia_y_offset: 0.0
mesh_package: panther_description
folder_path: meshes/WH02
kinematics: mecanum

velocity_x_stderr: 3.2e-3 # Selected intuitively, the need for thorough testing
velocity_y_stderr: 1.4e-3 # Selected intuitively, the need for thorough testing
velocity_yaw_stderr: 8.5e-3 # Selected intuitively, the need for thorough testing
rafal-gorecki marked this conversation as resolved.
Show resolved Hide resolved
4 changes: 4 additions & 0 deletions panther_description/config/WH04.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -6,3 +6,7 @@ inertia_y_offset: 0.0
mesh_package: panther_description
folder_path: meshes/WH04
kinematics: differential

velocity_x_stderr: 1.6e-3 # Selected intuitively, the need for thorough testing
velocity_y_stderr: 7.3e-4 # Selected intuitively, the need for thorough testing
velocity_yaw_stderr: 4.3e-3 # Selected intuitively, the need for thorough testing
11 changes: 10 additions & 1 deletion panther_driver/src/driver_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,9 @@ def __init__(self, name: str) -> None:
self._motor_torque_constant = rospy.get_param('~motor_torque_constant', 2.6149)
self._gear_ratio = rospy.get_param('~gear_ratio', 30.08)
self._encoder_resolution = rospy.get_param('~encoder_resolution', 400 * 4)
self._v_x_var = float(rospy.get_param('~velocity_x_stderr', 3.2e-3))**2
self._v_y_var = float(rospy.get_param('~velocity_y_stderr', 3.2e-3))**2
self._v_yaw_var = float(rospy.get_param('~velocity_yaw_stderr', 8.5e-3))**2

self._publish_tf = rospy.get_param('~publish_tf', True)
self._publish_odom = rospy.get_param('~publish_odometry', True)
Expand Down Expand Up @@ -170,7 +173,13 @@ def __init__(self, name: str) -> None:
if self._publish_odom:
self._odom_msg = Odometry()
self._odom_msg.pose.covariance = [0.1 if (i % 7) == 0 else 0.0 for i in range(36)]
self._odom_msg.twist.covariance = [0.1 if (i % 7) == 0 else 0.0 for i in range(36)]

twist_cov = [0.0] * 36
cov_value = [self._v_x_var, self._v_y_var, 1e6, 1e6, 1e6, self._v_yaw_var]
for i in range (len(cov_value)):
twist_cov[7*i] = cov_value[i]
rafal-gorecki marked this conversation as resolved.
Show resolved Hide resolved

self._odom_msg.twist.covariance = twist_cov
self._odom_msg.header.frame_id = self._odom_frame
self._odom_msg.child_frame_id = self._base_link_frame
self._odom_pub = rospy.Publisher('odom/wheels', Odometry, queue_size=1)
Expand Down