From 90a840a71bedbb21626e1052bd9a256f65635475 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Wed, 7 Jun 2023 22:19:23 +0200 Subject: [PATCH 01/18] ekf initial --- panther_bringup/config/ekf_config.yaml | 75 ++++++++++++++++++++++++++ panther_bringup/config/imu_config.yaml | 4 +- panther_bringup/launch/ekf.launch | 11 ++++ panther_driver/src/driver_node.py | 3 +- 4 files changed, 90 insertions(+), 3 deletions(-) create mode 100644 panther_bringup/config/ekf_config.yaml create mode 100644 panther_bringup/launch/ekf.launch diff --git a/panther_bringup/config/ekf_config.yaml b/panther_bringup/config/ekf_config.yaml new file mode 100644 index 000000000..bc1526b65 --- /dev/null +++ b/panther_bringup/config/ekf_config.yaml @@ -0,0 +1,75 @@ +# Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html + +frequency: 20 +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, + true, true, true, + false, false, false, + false, false, false, + true, true, true] +imu0_nodelay: false +imu0_differential: false +imu0_relative: true +imu0_queue_size: 5 + +imu0_remove_gravitational_acceleration: true + +print_diagnostics: true +debug: false +debug_out_file: /path/to/debug/file.txt + +process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, 0, 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.03, 0, 0, 0, 0, 0, 0, 0, 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.06, 0, 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, 0.025, 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, 0, 0, 0.01, 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, 0, 0, 0, 0, 0.02, 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, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] + +initial_estimate_covariance: [1e-6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6] diff --git a/panther_bringup/config/imu_config.yaml b/panther_bringup/config/imu_config.yaml index 8a1eed524..141e2d636 100644 --- a/panther_bringup/config/imu_config.yaml +++ b/panther_bringup/config/imu_config.yaml @@ -11,8 +11,8 @@ PhidgetsSpatialNodelet: # 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 + linear_acceleration_stdev: 0.12 # optional param linear_acceleration_stdev, default is 280ug + angular_velocity_stdev: 0.0084 # 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 # compass correction params (see http://www.phidgets.com/docs/1044_User_Guide) diff --git a/panther_bringup/launch/ekf.launch b/panther_bringup/launch/ekf.launch new file mode 100644 index 000000000..aef7d7578 --- /dev/null +++ b/panther_bringup/launch/ekf.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index 2932a6947..219ae391b 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -1,5 +1,6 @@ #!/usr/bin/python3 +import numpy as np import math from typing import List, Tuple, TypeVar from threading import Lock @@ -170,7 +171,7 @@ 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)] + self._odom_msg.twist.covariance = np.diag([1e-5, 1e-5, np.inf, np.inf, np.inf, 1e-4]) 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) From 356c41db978ad181872c9de5a3adb8508b7abdda Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Wed, 14 Jun 2023 19:07:09 +0200 Subject: [PATCH 02/18] ekf working on bag --- panther_bringup/config/ekf_config.yaml | 66 +++++++++++----------- panther_bringup/config/imu_config.yaml | 13 ++++- panther_bringup/config/panther_common.yaml | 2 +- panther_bringup/launch/bringup.launch | 4 ++ panther_bringup/launch/ekf.launch | 11 ---- panther_bringup/package.xml | 1 + panther_driver/src/driver_node.py | 2 +- 7 files changed, 50 insertions(+), 49 deletions(-) delete mode 100644 panther_bringup/launch/ekf.launch diff --git a/panther_bringup/config/ekf_config.yaml b/panther_bringup/config/ekf_config.yaml index bc1526b65..c5af4e8be 100644 --- a/panther_bringup/config/ekf_config.yaml +++ b/panther_bringup/config/ekf_config.yaml @@ -27,10 +27,10 @@ odom0_relative: true imu0: imu/data imu0_config: [false, false, false, - true, true, true, false, false, false, false, false, false, - true, true, true] + false, false, false, + true, true, false] imu0_nodelay: false imu0_differential: false imu0_relative: true @@ -38,38 +38,38 @@ imu0_queue_size: 5 imu0_remove_gravitational_acceleration: true -print_diagnostics: true +print_diagnostics: false debug: false debug_out_file: /path/to/debug/file.txt -process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0.05, 0, 0, 0, 0, 0, 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.03, 0, 0, 0, 0, 0, 0, 0, 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.06, 0, 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, 0.025, 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, 0, 0, 0.01, 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, 0, 0, 0, 0, 0.02, 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, 0, 0, 0, 0, 0.01, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] +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, 5e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 5e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 5e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 2e-6, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 2e-6, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 2e-6, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-3, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-3, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-3] -initial_estimate_covariance: [1e-6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1e-6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1e-6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-6] +initial_estimate_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, 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, 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, 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, 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] diff --git a/panther_bringup/config/imu_config.yaml b/panther_bringup/config/imu_config.yaml index 141e2d636..a4ef3b99b 100644 --- a/panther_bringup/config/imu_config.yaml +++ b/panther_bringup/config/imu_config.yaml @@ -1,9 +1,16 @@ 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 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) @@ -11,8 +18,8 @@ PhidgetsSpatialNodelet: # 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.12 # optional param linear_acceleration_stdev, default is 280ug - angular_velocity_stdev: 0.0084 # optional param angular_velocity_stdev, default is 0.095 deg/s + linear_acceleration_stdev: 1.2e-1 # optional param linear_acceleration_stdev, default is 280ug + angular_velocity_stdev: 9.5e-3 # 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 # compass correction params (see http://www.phidgets.com/docs/1044_User_Guide) diff --git a/panther_bringup/config/panther_common.yaml b/panther_bringup/config/panther_common.yaml index 4b4a3edd0..5db8d86d1 100644 --- a/panther_bringup/config/panther_common.yaml +++ b/panther_bringup/config/panther_common.yaml @@ -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 diff --git a/panther_bringup/launch/bringup.launch b/panther_bringup/launch/bringup.launch index d4d720570..07f1f5320 100644 --- a/panther_bringup/launch/bringup.launch +++ b/panther_bringup/launch/bringup.launch @@ -62,6 +62,10 @@ + + + + diff --git a/panther_bringup/launch/ekf.launch b/panther_bringup/launch/ekf.launch deleted file mode 100644 index aef7d7578..000000000 --- a/panther_bringup/launch/ekf.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/panther_bringup/package.xml b/panther_bringup/package.xml index f3cc2a974..fb73f22ac 100644 --- a/panther_bringup/package.xml +++ b/panther_bringup/package.xml @@ -24,5 +24,6 @@ panther_msgs panther_power_control phidgets_spatial + robot_localization \ No newline at end of file diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index 219ae391b..34ef201e9 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -171,7 +171,7 @@ 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 = np.diag([1e-5, 1e-5, np.inf, np.inf, np.inf, 1e-4]) + self._odom_msg.twist.covariance = np.diag([1e-5, 1e-5, 1e6, 1e6, 1e6, 7.2e-5]).flatten().tolist() 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) From e9430ce93a869b028ff3acd6e98f12e76def0917 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Wed, 14 Jun 2023 19:42:07 +0200 Subject: [PATCH 03/18] update --- panther_bringup/config/ekf_config.yaml | 2 +- panther_bringup/config/imu_config.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/panther_bringup/config/ekf_config.yaml b/panther_bringup/config/ekf_config.yaml index c5af4e8be..3de924483 100644 --- a/panther_bringup/config/ekf_config.yaml +++ b/panther_bringup/config/ekf_config.yaml @@ -29,7 +29,7 @@ imu0: imu/data imu0_config: [false, false, false, false, false, false, false, false, false, - false, false, false, + false, false, true, true, true, false] imu0_nodelay: false imu0_differential: false diff --git a/panther_bringup/config/imu_config.yaml b/panther_bringup/config/imu_config.yaml index a4ef3b99b..8b054a9c4 100644 --- a/panther_bringup/config/imu_config.yaml +++ b/panther_bringup/config/imu_config.yaml @@ -18,8 +18,8 @@ PhidgetsSpatialNodelet: # 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: 1.2e-1 # optional param linear_acceleration_stdev, default is 280ug - angular_velocity_stdev: 9.5e-3 # optional param angular_velocity_stdev, default is 0.095 deg/s + linear_acceleration_stdev: 5.0e-1 # optional param linear_acceleration_stdev, default is 280ug + angular_velocity_stdev: 6.0e-2 # 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 # compass correction params (see http://www.phidgets.com/docs/1044_User_Guide) From 11acf79a27ed6e3d19fd37fbfe2e90cbc2bc3979 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 15 Jun 2023 14:22:29 +0200 Subject: [PATCH 04/18] atfer panther tests --- panther_bringup/config/imu_config.yaml | 4 ++-- panther_bringup/launch/bringup.launch | 2 +- panther_bringup/launch/ekf.launch | 11 +++++++++++ 3 files changed, 14 insertions(+), 3 deletions(-) create mode 100644 panther_bringup/launch/ekf.launch diff --git a/panther_bringup/config/imu_config.yaml b/panther_bringup/config/imu_config.yaml index 8b054a9c4..b7437dbe8 100644 --- a/panther_bringup/config/imu_config.yaml +++ b/panther_bringup/config/imu_config.yaml @@ -18,8 +18,8 @@ PhidgetsSpatialNodelet: # 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: 5.0e-1 # optional param linear_acceleration_stdev, default is 280ug - angular_velocity_stdev: 6.0e-2 # optional param angular_velocity_stdev, default is 0.095 deg/s + linear_acceleration_stdev: 2.4e-1 # optional param linear_acceleration_stdev, default is 280ug + angular_velocity_stdev: 3.0e-2 # 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 # compass correction params (see http://www.phidgets.com/docs/1044_User_Guide) diff --git a/panther_bringup/launch/bringup.launch b/panther_bringup/launch/bringup.launch index 07f1f5320..935de761d 100644 --- a/panther_bringup/launch/bringup.launch +++ b/panther_bringup/launch/bringup.launch @@ -62,7 +62,7 @@ - + diff --git a/panther_bringup/launch/ekf.launch b/panther_bringup/launch/ekf.launch new file mode 100644 index 000000000..9edc87a4f --- /dev/null +++ b/panther_bringup/launch/ekf.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file From dfa4d069c711f17c0e3d48c73f85259d485cb252 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 20 Jun 2023 12:53:58 +0200 Subject: [PATCH 05/18] bringup - include ekf --- panther_bringup/launch/bringup.launch | 4 +--- panther_bringup/launch/ekf.launch | 4 ---- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/panther_bringup/launch/bringup.launch b/panther_bringup/launch/bringup.launch index 935de761d..48c078ca1 100644 --- a/panther_bringup/launch/bringup.launch +++ b/panther_bringup/launch/bringup.launch @@ -62,9 +62,7 @@ - - - + diff --git a/panther_bringup/launch/ekf.launch b/panther_bringup/launch/ekf.launch index 9edc87a4f..5dee16fa9 100644 --- a/panther_bringup/launch/ekf.launch +++ b/panther_bringup/launch/ekf.launch @@ -1,11 +1,7 @@ - - - - \ No newline at end of file From 71d9c38937a5666f7c1b4e13e195a7aba52e6379 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 4 Jul 2023 16:14:24 +0200 Subject: [PATCH 06/18] Maciej suggestions --- panther_bringup/config/ekf_config.yaml | 40 +++++++++----------------- panther_bringup/config/imu_config.yaml | 8 +++--- panther_description/config/WH01.yaml | 4 +++ panther_description/config/WH02.yaml | 4 +++ panther_description/config/WH04.yaml | 4 +++ panther_driver/src/driver_node.py | 15 +++++++++- 6 files changed, 43 insertions(+), 32 deletions(-) diff --git a/panther_bringup/config/ekf_config.yaml b/panther_bringup/config/ekf_config.yaml index 3de924483..dc2ce34dc 100644 --- a/panther_bringup/config/ekf_config.yaml +++ b/panther_bringup/config/ekf_config.yaml @@ -1,6 +1,6 @@ # Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html -frequency: 20 +frequency: 15 sensor_timeout: 0.2 two_d_mode: true @@ -34,42 +34,28 @@ imu0_config: [false, false, false, imu0_nodelay: false imu0_differential: false imu0_relative: true -imu0_queue_size: 5 +imu0_queue_size: 10 imu0_remove_gravitational_acceleration: true +reset_on_time_jump: false print_diagnostics: false debug: false debug_out_file: /path/to/debug/file.txt -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, +# Selected values ​​experimentally so as to ensure relatively fast convergence (values ​​should be about 10x higher than the sensor variacne values) +process_noise_covariance: [2e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 2e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 2e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 2e-6, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 2e-6, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 2e-6, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-3, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-3, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-3] - -initial_estimate_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, 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, 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, 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, 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, 5e-1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-1] diff --git a/panther_bringup/config/imu_config.yaml b/panther_bringup/config/imu_config.yaml index b7437dbe8..1b89327ad 100644 --- a/panther_bringup/config/imu_config.yaml +++ b/panther_bringup/config/imu_config.yaml @@ -7,7 +7,7 @@ ImuFilterNodelet: orientation_stddev: 1e-2 publish_tf: false use_mag: false - use_magnetic_field_msg: true + 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. fixed_frame: imu_link stateless: false remove_gravity_vector: false @@ -18,9 +18,9 @@ PhidgetsSpatialNodelet: # 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: 2.4e-1 # optional param linear_acceleration_stdev, default is 280ug - angular_velocity_stdev: 3.0e-2 # 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) # compass correction params (see http://www.phidgets.com/docs/1044_User_Guide) # cc_mag_field: 0.52859 diff --git a/panther_description/config/WH01.yaml b/panther_description/config/WH01.yaml index 0f4dc7353..4da1a9c1b 100644 --- a/panther_description/config/WH01.yaml +++ b/panther_description/config/WH01.yaml @@ -6,3 +6,7 @@ inertia_y_offset: 0.0 mesh_package: panther_description folder_path: meshes/WH01 kinematics: differential + +velocity_x_stderr: 3.2e-2 +velocity_y_stderr: 3.2e-2 +velocity_yaw_stderr: 8.5e-3 \ No newline at end of file diff --git a/panther_description/config/WH02.yaml b/panther_description/config/WH02.yaml index d25f0ae71..c0e3576b2 100644 --- a/panther_description/config/WH02.yaml +++ b/panther_description/config/WH02.yaml @@ -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: 3.2e-3 # Selected intuitively, the need for thorough testing +velocity_yaw_stderr: 8.5e-3 # Selected intuitively, the need for thorough testing \ No newline at end of file diff --git a/panther_description/config/WH04.yaml b/panther_description/config/WH04.yaml index bfc64e845..bcc084c39 100644 --- a/panther_description/config/WH04.yaml +++ b/panther_description/config/WH04.yaml @@ -6,3 +6,7 @@ inertia_y_offset: 0.0 mesh_package: panther_description folder_path: meshes/WH04 kinematics: differential + +velocity_x_stderr: 2.2e-3 # Selected intuitively, the need for thorough testing +velocity_y_stderr: 2.2e-3 # Selected intuitively, the need for thorough testing +velocity_yaw_stderr: 8.5e-3 # Selected intuitively, the need for thorough testing \ No newline at end of file diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index 34ef201e9..76a633b37 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -76,6 +76,10 @@ 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_std = float(rospy.get_param('~velocity_x_stderr', 3.2e-3))**2 + self._v_y_std = float(rospy.get_param('~velocity_y_stderr', 3.2e-3))**2 + self._v_yaw_std = float(rospy.get_param('~velocity_yaw_stderr', 8.5e-3))**2 + print(self._v_x_std) self._publish_tf = rospy.get_param('~publish_tf', True) self._publish_odom = rospy.get_param('~publish_odometry', True) @@ -171,7 +175,16 @@ 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 = np.diag([1e-5, 1e-5, 1e6, 1e6, 1e6, 7.2e-5]).flatten().tolist() + + twist_cov = [0.0] * 36 + twist_cov[0] = self._v_x_std + twist_cov[7] = self._v_y_std + twist_cov[14] = 1e6 + twist_cov[21] = 1e6 + twist_cov[28] = 1e6 + twist_cov[35] = self._v_yaw_std + + 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) From 004c5d855087d69d070326361d278028bddd9640 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Wed, 5 Jul 2023 13:33:05 +0200 Subject: [PATCH 07/18] Add dynamic process noice --- panther_bringup/config/ekf_config.yaml | 1 + panther_description/config/WH01.yaml | 4 ++-- panther_description/config/WH02.yaml | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/panther_bringup/config/ekf_config.yaml b/panther_bringup/config/ekf_config.yaml index dc2ce34dc..09ca44f19 100644 --- a/panther_bringup/config/ekf_config.yaml +++ b/panther_bringup/config/ekf_config.yaml @@ -44,6 +44,7 @@ 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) +dynamic_process_noise_covariance: true process_noise_covariance: [2e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, diff --git a/panther_description/config/WH01.yaml b/panther_description/config/WH01.yaml index 4da1a9c1b..d9a47b157 100644 --- a/panther_description/config/WH01.yaml +++ b/panther_description/config/WH01.yaml @@ -7,6 +7,6 @@ mesh_package: panther_description folder_path: meshes/WH01 kinematics: differential -velocity_x_stderr: 3.2e-2 -velocity_y_stderr: 3.2e-2 +velocity_x_stderr: 3.2e-3 +velocity_y_stderr: 3.2e-3 velocity_yaw_stderr: 8.5e-3 \ No newline at end of file diff --git a/panther_description/config/WH02.yaml b/panther_description/config/WH02.yaml index c0e3576b2..85569a400 100644 --- a/panther_description/config/WH02.yaml +++ b/panther_description/config/WH02.yaml @@ -7,6 +7,6 @@ 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: 3.2e-3 # Selected intuitively, the need for thorough testing +velocity_x_stderr: 3.2e-2 # Selected intuitively, the need for thorough testing +velocity_y_stderr: 3.2e-2 # Selected intuitively, the need for thorough testing velocity_yaw_stderr: 8.5e-3 # Selected intuitively, the need for thorough testing \ No newline at end of file From f22d0972623313d7484c2fc7c25900c1d60baaa7 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 6 Jul 2023 02:55:48 +0200 Subject: [PATCH 08/18] Final ekf config --- panther_bringup/config/ekf_config.yaml | 35 +++++++++++++------------- panther_description/config/WH02.yaml | 4 +-- panther_description/config/WH04.yaml | 6 ++--- 3 files changed, 23 insertions(+), 22 deletions(-) diff --git a/panther_bringup/config/ekf_config.yaml b/panther_bringup/config/ekf_config.yaml index 09ca44f19..7e59411bb 100644 --- a/panther_bringup/config/ekf_config.yaml +++ b/panther_bringup/config/ekf_config.yaml @@ -1,6 +1,6 @@ # Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html -frequency: 15 +frequency: 20 sensor_timeout: 0.2 two_d_mode: true @@ -34,29 +34,30 @@ imu0_config: [false, false, false, imu0_nodelay: false imu0_differential: false imu0_relative: true -imu0_queue_size: 10 +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) dynamic_process_noise_covariance: true -process_noise_covariance: [2e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 2e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 2e-4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 5e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 5e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 5e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-4, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5e-1] +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] diff --git a/panther_description/config/WH02.yaml b/panther_description/config/WH02.yaml index 85569a400..272f04a3b 100644 --- a/panther_description/config/WH02.yaml +++ b/panther_description/config/WH02.yaml @@ -7,6 +7,6 @@ mesh_package: panther_description folder_path: meshes/WH02 kinematics: mecanum -velocity_x_stderr: 3.2e-2 # Selected intuitively, the need for thorough testing -velocity_y_stderr: 3.2e-2 # Selected intuitively, the need for thorough testing +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 \ No newline at end of file diff --git a/panther_description/config/WH04.yaml b/panther_description/config/WH04.yaml index bcc084c39..650c287c8 100644 --- a/panther_description/config/WH04.yaml +++ b/panther_description/config/WH04.yaml @@ -7,6 +7,6 @@ mesh_package: panther_description folder_path: meshes/WH04 kinematics: differential -velocity_x_stderr: 2.2e-3 # Selected intuitively, the need for thorough testing -velocity_y_stderr: 2.2e-3 # Selected intuitively, the need for thorough testing -velocity_yaw_stderr: 8.5e-3 # Selected intuitively, the need for thorough testing \ No newline at end of file +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 \ No newline at end of file From dee52b126b4c2cb808a6cb3b8331952652df951b Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 6 Jul 2023 02:56:53 +0200 Subject: [PATCH 09/18] wheek param --- panther_description/config/WH01.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_description/config/WH01.yaml b/panther_description/config/WH01.yaml index d9a47b157..d089db763 100644 --- a/panther_description/config/WH01.yaml +++ b/panther_description/config/WH01.yaml @@ -8,5 +8,5 @@ folder_path: meshes/WH01 kinematics: differential velocity_x_stderr: 3.2e-3 -velocity_y_stderr: 3.2e-3 +velocity_y_stderr: 1.4e-3 velocity_yaw_stderr: 8.5e-3 \ No newline at end of file From 2b4059e34d09e3d2a7dec1fbd858aea894fb61d6 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 6 Jul 2023 02:59:02 +0200 Subject: [PATCH 10/18] import --- panther_driver/src/driver_node.py | 1 - 1 file changed, 1 deletion(-) diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index 76a633b37..110cbcbb2 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -1,6 +1,5 @@ #!/usr/bin/python3 -import numpy as np import math from typing import List, Tuple, TypeVar from threading import Lock From 02570bcaca3ff56aacf197ec24111faebf4b4366 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 6 Jul 2023 03:07:38 +0200 Subject: [PATCH 11/18] refactor --- panther_driver/src/driver_node.py | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index 110cbcbb2..3903b2e21 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -75,10 +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_std = float(rospy.get_param('~velocity_x_stderr', 3.2e-3))**2 - self._v_y_std = float(rospy.get_param('~velocity_y_stderr', 3.2e-3))**2 - self._v_yaw_std = float(rospy.get_param('~velocity_yaw_stderr', 8.5e-3))**2 - print(self._v_x_std) + 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) @@ -176,12 +175,9 @@ def __init__(self, name: str) -> None: self._odom_msg.pose.covariance = [0.1 if (i % 7) == 0 else 0.0 for i in range(36)] twist_cov = [0.0] * 36 - twist_cov[0] = self._v_x_std - twist_cov[7] = self._v_y_std - twist_cov[14] = 1e6 - twist_cov[21] = 1e6 - twist_cov[28] = 1e6 - twist_cov[35] = self._v_yaw_std + 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] self._odom_msg.twist.covariance = twist_cov self._odom_msg.header.frame_id = self._odom_frame From f83e451ce4acf8d273f4566f6885ccadbcd1dee6 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Fri, 7 Jul 2023 11:54:58 +0200 Subject: [PATCH 12/18] Update panther_bringup/config/ekf_config.yaml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Maciej Stępień --- panther_bringup/config/ekf_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_bringup/config/ekf_config.yaml b/panther_bringup/config/ekf_config.yaml index 7e59411bb..d7e00d79b 100644 --- a/panther_bringup/config/ekf_config.yaml +++ b/panther_bringup/config/ekf_config.yaml @@ -44,7 +44,7 @@ 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) +# Selected values ​​experimentally so as to ensure relatively fast convergence (values ​​should be about 10x higher than the sensor variance values) 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, From 34842dfeaacb050ea8ed210b9e431f9e68f2bd2d Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Fri, 7 Jul 2023 11:55:45 +0200 Subject: [PATCH 13/18] Update panther_bringup/config/imu_config.yaml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Maciej Stępień --- panther_bringup/config/imu_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_bringup/config/imu_config.yaml b/panther_bringup/config/imu_config.yaml index 1b89327ad..fc0e46f33 100644 --- a/panther_bringup/config/imu_config.yaml +++ b/panther_bringup/config/imu_config.yaml @@ -7,7 +7,7 @@ ImuFilterNodelet: orientation_stddev: 1e-2 publish_tf: false use_mag: 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. + 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. Only changes type of the message, whether mag data is used is controlled by the use_mag parameter fixed_frame: imu_link stateless: false remove_gravity_vector: false From 58cb931ee2dcdfae5cba55f428d5ae9929b9bdfd Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Fri, 7 Jul 2023 11:56:47 +0200 Subject: [PATCH 14/18] Update panther_bringup/config/imu_config.yaml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Maciej Stępień --- panther_bringup/config/imu_config.yaml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/panther_bringup/config/imu_config.yaml b/panther_bringup/config/imu_config.yaml index fc0e46f33..1e51fd0a1 100644 --- a/panther_bringup/config/imu_config.yaml +++ b/panther_bringup/config/imu_config.yaml @@ -18,9 +18,10 @@ PhidgetsSpatialNodelet: # 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: 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) + # stdev values from imu specification (https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1205) + linear_acceleration_stdev: 3.5e-3 # Currently used IMU specification 10 * (2mg ± 1.5mg) (Accelerometer Drift Max ± Accelerometer Noise for 100 Hz) + angular_velocity_stdev: 1.5e-2 # Currently used IMU specification 0.1°/s ± 0.05°/s (Gyroscope Drift Max ± Gyroscope Noise for 100 Hz) + # magnetic_field_stdev: 10.0 # Currently used IMU specification ± 10mG (± Magnetometer Noise) # compass correction params (see http://www.phidgets.com/docs/1044_User_Guide) # cc_mag_field: 0.52859 From bbda67273b55b44784d7c99e25ca72509ebf8b4a Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 7 Jul 2023 12:18:55 +0200 Subject: [PATCH 15/18] Sugestions --- panther_description/config/WH01.yaml | 6 +++--- panther_description/config/WH02.yaml | 6 +++--- panther_description/config/WH04.yaml | 6 +++--- panther_driver/src/driver_node.py | 13 +++++++------ 4 files changed, 16 insertions(+), 15 deletions(-) diff --git a/panther_description/config/WH01.yaml b/panther_description/config/WH01.yaml index d089db763..8b12eed40 100644 --- a/panther_description/config/WH01.yaml +++ b/panther_description/config/WH01.yaml @@ -7,6 +7,6 @@ mesh_package: panther_description folder_path: meshes/WH01 kinematics: differential -velocity_x_stderr: 3.2e-3 -velocity_y_stderr: 1.4e-3 -velocity_yaw_stderr: 8.5e-3 \ No newline at end of file +velocity_x_stderr: 3.2e-3 # Value measured (using Husarion research toolkit) +velocity_y_stderr: 1.4e-3 # Value measured (using Husarion research toolkit) +velocity_yaw_stderr: 8.5e-3 # Value measured (using Husarion research toolkit) \ No newline at end of file diff --git a/panther_description/config/WH02.yaml b/panther_description/config/WH02.yaml index 272f04a3b..883d87793 100644 --- a/panther_description/config/WH02.yaml +++ b/panther_description/config/WH02.yaml @@ -7,6 +7,6 @@ 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 \ No newline at end of file +velocity_x_stderr: 3.2e-3 # Selected intuitively based on WH01 results +velocity_y_stderr: 3.2e-3 # Selected intuitively based on WH01 results +velocity_yaw_stderr: 8.5e-3 # Selected intuitively based on WH01 results \ No newline at end of file diff --git a/panther_description/config/WH04.yaml b/panther_description/config/WH04.yaml index 650c287c8..c5763fd33 100644 --- a/panther_description/config/WH04.yaml +++ b/panther_description/config/WH04.yaml @@ -7,6 +7,6 @@ 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 \ No newline at end of file +velocity_x_stderr: 1.6e-3 # Selected intuitively based on WH01 results +velocity_y_stderr: 7.3e-4 # Selected intuitively based on WH01 results +velocity_yaw_stderr: 4.3e-3 # Selected intuitively based on WH01 results \ No newline at end of file diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index 3903b2e21..4391a4d7d 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -174,12 +174,13 @@ def __init__(self, name: str) -> None: self._odom_msg = Odometry() self._odom_msg.pose.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] - - self._odom_msg.twist.covariance = twist_cov + not_used_var = 1e6 + self._odom_msg.twist.covariance = [self._v_x_var, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, self._v_y_var, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, not_used_var, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, not_used_var, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, not_used_var, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, self._v_yaw_var] 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) From e51102f6303bc5e6a8cf46fee74d8cd1d8586f34 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Fri, 7 Jul 2023 13:16:30 +0200 Subject: [PATCH 16/18] Update panther_description/config/WH01.yaml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Maciej Stępień --- panther_description/config/WH01.yaml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/panther_description/config/WH01.yaml b/panther_description/config/WH01.yaml index 8b12eed40..3097414d8 100644 --- a/panther_description/config/WH01.yaml +++ b/panther_description/config/WH01.yaml @@ -7,6 +7,7 @@ mesh_package: panther_description folder_path: meshes/WH01 kinematics: differential -velocity_x_stderr: 3.2e-3 # Value measured (using Husarion research toolkit) -velocity_y_stderr: 1.4e-3 # Value measured (using Husarion research toolkit) -velocity_yaw_stderr: 8.5e-3 # Value measured (using Husarion research toolkit) \ No newline at end of file +# Value measured (using Husarion research toolkit) +velocity_x_stderr: 3.2e-3 +velocity_y_stderr: 1.4e-3 +velocity_yaw_stderr: 8.5e-3 \ No newline at end of file From 3a67543fe1fa72d28b86c430d5aeea7198dc6af9 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Fri, 7 Jul 2023 13:16:36 +0200 Subject: [PATCH 17/18] Update panther_description/config/WH02.yaml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Maciej Stępień --- panther_description/config/WH02.yaml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/panther_description/config/WH02.yaml b/panther_description/config/WH02.yaml index 883d87793..582646916 100644 --- a/panther_description/config/WH02.yaml +++ b/panther_description/config/WH02.yaml @@ -7,6 +7,7 @@ mesh_package: panther_description folder_path: meshes/WH02 kinematics: mecanum -velocity_x_stderr: 3.2e-3 # Selected intuitively based on WH01 results -velocity_y_stderr: 3.2e-3 # Selected intuitively based on WH01 results -velocity_yaw_stderr: 8.5e-3 # Selected intuitively based on WH01 results \ No newline at end of file +# Selected intuitively based on WH01 results +velocity_x_stderr: 3.2e-3 +velocity_y_stderr: 3.2e-3 +velocity_yaw_stderr: 8.5e-3 \ No newline at end of file From 23c2bf1173638840f1297560a727b5b1b4bef947 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Fri, 7 Jul 2023 13:16:45 +0200 Subject: [PATCH 18/18] Update panther_description/config/WH04.yaml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Maciej Stępień --- panther_description/config/WH04.yaml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/panther_description/config/WH04.yaml b/panther_description/config/WH04.yaml index c5763fd33..340ade38e 100644 --- a/panther_description/config/WH04.yaml +++ b/panther_description/config/WH04.yaml @@ -7,6 +7,7 @@ mesh_package: panther_description folder_path: meshes/WH04 kinematics: differential -velocity_x_stderr: 1.6e-3 # Selected intuitively based on WH01 results -velocity_y_stderr: 7.3e-4 # Selected intuitively based on WH01 results -velocity_yaw_stderr: 4.3e-3 # Selected intuitively based on WH01 results \ No newline at end of file +# Selected intuitively based on WH01 results +velocity_x_stderr: 1.6e-3 +velocity_y_stderr: 7.3e-4 +velocity_yaw_stderr: 4.3e-3 \ No newline at end of file