diff --git a/panther_description/README.md b/panther_description/README.md index 07c64c545..85cae0245 100644 --- a/panther_description/README.md +++ b/panther_description/README.md @@ -38,6 +38,10 @@ Parameter `wheel_config_path` allows using non-standard wheels with Panther robo - `mesh_package` - ROS package name to search for custom meshes. Used in evaluation **$(find my_amazing_package)/**. - `folder_path` - path used to search for mesh files within the ROS package. - `kinematics` - kinematics type. Possible options: `differential`, `mecanum`. +- `odom_stderr` - standard deviation used to populated odometry message: + - `vel_x` - standard deviation for linear velocity in **x** direction. + - `vel_y` - standard deviation for linear velocity in **y** direction. + - `vel_yaw` - standard deviation for angular velocity alongside **z** axis. Wheels have to be named as follows: - `wheel_collision.stl` - wheel collision mesh. diff --git a/panther_description/config/WH01.yaml b/panther_description/config/WH01.yaml index 3097414d8..11312fad2 100644 --- a/panther_description/config/WH01.yaml +++ b/panther_description/config/WH01.yaml @@ -7,7 +7,5 @@ mesh_package: panther_description folder_path: meshes/WH01 kinematics: differential -# 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 +# Values measured experimentally +odom_stderr: { vel_x: 3.2e-3, vel_y: 1.4e-3, vel_yaw: 8.5e-3 } diff --git a/panther_description/config/WH02.yaml b/panther_description/config/WH02.yaml index 582646916..2406fb454 100644 --- a/panther_description/config/WH02.yaml +++ b/panther_description/config/WH02.yaml @@ -8,6 +8,4 @@ folder_path: meshes/WH02 kinematics: mecanum # 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 +odom_stderr: { vel_x: 3.2e-3, vel_y: 3.2e-3, vel_yaw: 8.5e-3 } diff --git a/panther_description/config/WH04.yaml b/panther_description/config/WH04.yaml index 340ade38e..fe2ee546a 100644 --- a/panther_description/config/WH04.yaml +++ b/panther_description/config/WH04.yaml @@ -8,6 +8,4 @@ folder_path: meshes/WH04 kinematics: differential # 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 +odom_stderr: { vel_x: 1.6e-3, vel_y: 7.3e-4, vel_yaw: 4.3e-3 } \ No newline at end of file diff --git a/panther_driver/README.md b/panther_driver/README.md index 6a7c1bdbd..39cb2e750 100644 --- a/panther_driver/README.md +++ b/panther_driver/README.md @@ -44,6 +44,9 @@ For a `/joint_states` message is carrying given data: - `~kinematics` [*string*, default: **differential**]: kinematics type, possible are: differential, mecanum. - `~motor_torque_constant` [*float*, default: **2.6149**]: constant used to estimate torque. - `~odom_frame` [*string*, default: **odom**]: the name of the odom frame. +- `~odom_stderr/vel_x` [*float*, default: **3.2e-3**]: standard error used to place in covariance matrix, after squaring, in odometry message. +- `~odom_stderr/vel_y` [*float*, default: **3.2e-3**]: standard error used to place in covariance matrix, after squaring, in odometry message. +- `~odom_stderr/vel_yaw` [*float*, default: **8.5e-3**]: standard error used to place in covariance matrix, after squaring, in odometry message. - `~publish_joints` [*bool*, default: **true**]: whether to publish robot joints states. - `~publish_odometry` [*bool*, default: **true**]: whether to publish robot odometry. - `~publish_pose` [*bool*, default: **true**]: whether to publish robot pose. diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index ce9e518b1..f1542de07 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -76,9 +76,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._v_x_var = float(rospy.get_param('~odom_stderr/vel_x', 3.2e-3))**2 + self._v_y_var = float(rospy.get_param('~odom_stderr/vel_y', 3.2e-3))**2 + self._v_yaw_var = float(rospy.get_param('~odom_stderr/vel_yaw', 8.5e-3))**2 self._publish_tf = rospy.get_param('~publish_tf', True) self._publish_odom = rospy.get_param('~publish_odometry', True)