From 96c69e859cf74008c3ed61de1bdb7672b6ec74b0 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Fri, 3 Nov 2023 16:13:01 +0100 Subject: [PATCH] Tried to fix ukf --- src/dcaiti_control/CMakeLists.txt | 10 +- src/dcaiti_control/config/ukf.yml | 217 ++++++++++++++++++ src/dcaiti_control/dcaiti_control/__init__.py | 0 src/dcaiti_control/description/imu.xacro | 47 ++++ src/dcaiti_control/launch/launch_sim.py | 27 ++- src/dcaiti_control/launch/rsp.launch.py | 2 +- src/dcaiti_control/src/__init__.py | 0 .../src/imu_covariance_adder.py | 51 ++++ 8 files changed, 351 insertions(+), 3 deletions(-) create mode 100644 src/dcaiti_control/config/ukf.yml create mode 100644 src/dcaiti_control/dcaiti_control/__init__.py create mode 100644 src/dcaiti_control/src/__init__.py create mode 100755 src/dcaiti_control/src/imu_covariance_adder.py diff --git a/src/dcaiti_control/CMakeLists.txt b/src/dcaiti_control/CMakeLists.txt index 0875add..ae34637 100644 --- a/src/dcaiti_control/CMakeLists.txt +++ b/src/dcaiti_control/CMakeLists.txt @@ -33,9 +33,17 @@ if(BUILD_TESTING) endif() install( - DIRECTORY config description launch worlds + DIRECTORY config description launch worlds src DESTINATION share/${PROJECT_NAME} ) +ament_python_install_package(${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + src/imu_covariance_adder.py + DESTINATION lib/${PROJECT_NAME} +) + ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.dsv.in") ament_package() diff --git a/src/dcaiti_control/config/ukf.yml b/src/dcaiti_control/config/ukf.yml new file mode 100644 index 0000000..a8d0227 --- /dev/null +++ b/src/dcaiti_control/config/ukf.yml @@ -0,0 +1,217 @@ +### ukf config file ### +ukf_filter_node: + ros__parameters: + # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin + # computation until it receives at least one message from one of the inputs. It will then run continuously at the + # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. + frequency: 30.0 + + # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict + # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the + # filter will generate new output. Defaults to 1 / frequency if not specified. + sensor_timeout: 0.1 + + # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is + # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar + # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected + # by, for example, an IMU. Defaults to false if unspecified. + two_d_mode: True + + # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for + # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if + # unspecified. + transform_time_offset: 0.0 + + # Use this parameter to provide specify how long the tf listener should wait for a transform to become available. + # Defaults to 0.0 if unspecified. + transform_timeout: 0.0 + + # If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is + # unhappy with any settings or data. + print_diagnostics: true + + # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by + # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious + # effects on the performance of the node. Defaults to false if unspecified. + debug: false + + # Whether we'll allow old measurements to cause a re-publication of the updated state + permit_corrected_publication: false + + # Whether to publish the acceleration state. Defaults to false if unspecified. + publish_acceleration: false + + # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. + publish_tf: true + + # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and + # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. + # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be + # continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom + # frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your + # robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based + # localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. + # ekf_localization_node and ukf_localization_node are not concerned with the earth frame. + # Here is how to use the following settings: + # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. + # 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of + # odom_frame. + # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set + # "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. + # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates + # from landmark observations) then: + # 3a. Set your "world_frame" to your map_frame value + # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state + # estimation node from robot_localization! However, that instance should *not* fuse the global data. + map_frame: map # Defaults to "map" if unspecified + odom_frame: odom # Defaults to "odom" if unspecified + base_link_frame: base_link # Defaults to "base_link" if unspecified + world_frame: odom # Defaults to the value of odom_frame if unspecified + + # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, + # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, + # sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, + # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no + # default values, and must be specified. + odom0: /ackermann_steering_controller/odometry + + # Each sensor reading updates some or all of the filter's state. These options give you greater control over which + # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only + # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the + # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types + # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message + # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false + # if unspecified, effectively making this parameter required for each sensor. + odom0_config: [false, false, false, + false, false, false, + true, false, false, + false, false, true, + false, false, false] + + # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase + # the size of the subscription queue so that more measurements are fused. + odom0_queue_size: 2 + + # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- + # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they + # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also + # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't + # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose + # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then + # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true + # for twist measurements has no effect. + odom0_differential: false + + # [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" + # for all future measurements. While you can achieve the same effect with the differential paremeter, the key + # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before + # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. + odom0_relative: false + + # [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to + # control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to + # numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not + # required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. + # For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying + # the thresholds. + odom0_pose_rejection_threshold: 5.0 + odom0_twist_rejection_threshold: 1.0 + + #imu0: lidar/imu_covariance + #imu0_config: [false, false, false, + # true, true, true, + # false, false, false, + # true, true, true, + # true, true, true] + #imu0_differential: false + #imu0_relative: true + #imu0_queue_size: 5 + #imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names + #imu0_twist_rejection_threshold: 0.8 # + #imu0_linear_acceleration_rejection_threshold: 0.8 # + + # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set + # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. + #imu0_remove_gravitational_acceleration: true + + # [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no + # acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During + # correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be + # problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When + # this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially + # noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance + # for the velocity variable in question, or decrease the variance of the variable in question in the measurement + # itself. In addition, users can also take advantage of the control command being issued to the robot at the time we + # make the prediction. If control is used, it will get converted into an acceleration term, which will be used during + # predicition. Note that if an acceleration measurement for the variable in question is available from one of the + # inputs, the control term will be ignored. + # Whether or not we use the control input during predicition. Defaults to false. + use_control: false + + # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to + # false. + stamped_control: false + + # The last issued control command will be used in prediction for this period. Defaults to 0.2. + control_timeout: 0.2 + + # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. + control_config: [true, false, false, false, false, true] + + # Places limits on how large the acceleration term will be. Should match your robot's kinematics. + acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] + + # Acceleration and deceleration limits are not always the same for robots. + deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] + + # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these + # gains + acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] + + # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these + # gains + deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] + + # [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. + # Note: the specification of covariance matrices can be cumbersome, so all matrix parameters in this package support + # both full specification or specification of only the diagonal values. + 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.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.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.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.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.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.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.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, 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, 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, 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, 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, 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, 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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + + # [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 diagonal values below + # if unspecified. In this example, we specify only the diagonal of the matrix. + initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9] + + # [ADVANCED, UKF ONLY] The alpha and kappa variables control the spread of the sigma points. Unless you are familiar + # with UKFs, it's probably a good idea to leave these alone. + # Defaults to 0.001 if unspecified. + alpha: 0.001 + # Defaults to 0 if unspecified. + kappa: 0.0 + + # [ADVANCED, UKF ONLY] The beta variable relates to the distribution of the state vector. Again, it's probably best to + # leave this alone if you're uncertain. Defaults to 2 if unspecified. + beta: 2.0 \ No newline at end of file diff --git a/src/dcaiti_control/dcaiti_control/__init__.py b/src/dcaiti_control/dcaiti_control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/dcaiti_control/description/imu.xacro b/src/dcaiti_control/description/imu.xacro index e2e5aa9..249c19c 100644 --- a/src/dcaiti_control/description/imu.xacro +++ b/src/dcaiti_control/description/imu.xacro @@ -3,10 +3,57 @@ Gazebo/Red + laser_frame 1 50 false /lidar/imu + + + + gaussian + 0.0 + 0.01 + + + + + gaussian + 0.0 + 0.01 + + + + + gaussian + 0.0 + 0.01 + + + + + + + gaussian + 0.0 + 0.01 + + + + + gaussian + 0.0 + 0.01 + + + + + gaussian + 0.0 + 0.01 + + + \ No newline at end of file diff --git a/src/dcaiti_control/launch/launch_sim.py b/src/dcaiti_control/launch/launch_sim.py index 181c68e..dc2ee5e 100644 --- a/src/dcaiti_control/launch/launch_sim.py +++ b/src/dcaiti_control/launch/launch_sim.py @@ -41,7 +41,7 @@ def generate_launch_description(): gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')]), - launch_arguments=[('gz_args', [f" -r -v 1 {world_path}/generated_worlds/AU2_skidpad.sdf"])], + launch_arguments=[('gz_args', [f" -r -v 0 {world_path}/generated_worlds/AU2_skidpad.sdf"])], ) # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot. @@ -94,6 +94,29 @@ def generate_launch_description(): on_exit=[diff_drive_spawner], ) ) + + imu_covariance_adder = Node( + package='dcaiti_control', + executable='imu_covariance_adder.py', + name='imu_covariance_adder', + output='screen', + parameters=[ + {'orientation_covariance': [1e-3]*9}, + {'linear_acceleration_covariance': [1e-3]*9}, + {'angular_velocity_covariance': [1e-3]*9}, + {'subscribe_topic': '/lidar/imu'}, + {'publish_topic': '/lidar/imu_covariance'} + ] + ) + + #ukf_node = Node( + # package='robot_localization', + # executable='ukf_node', + # name='ukf_filter_node', + # output='screen', + # parameters=[str(base_path / 'config' / 'ukf.yml')], + # remappings=[('odometry/filtered', 'odometry/local')] + # ) # Launch them all! @@ -112,4 +135,6 @@ def generate_launch_description(): spawn_entity, delayed_diff_drive_spawner, delayed_joint_broad_spawner, + imu_covariance_adder, + #ukf_node ]) diff --git a/src/dcaiti_control/launch/rsp.launch.py b/src/dcaiti_control/launch/rsp.launch.py index eaa5549..4e25181 100644 --- a/src/dcaiti_control/launch/rsp.launch.py +++ b/src/dcaiti_control/launch/rsp.launch.py @@ -67,5 +67,5 @@ def generate_launch_description(): node_robot_state_publisher, node_joint_state_publisher, - twist_mux, + #twist_mux, ]) diff --git a/src/dcaiti_control/src/__init__.py b/src/dcaiti_control/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/dcaiti_control/src/imu_covariance_adder.py b/src/dcaiti_control/src/imu_covariance_adder.py new file mode 100755 index 0000000..6ef6943 --- /dev/null +++ b/src/dcaiti_control/src/imu_covariance_adder.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Imu + +class ImuCovarianceNode(Node): + def __init__(self): + super().__init__('imu_covariance_node') + + self.declare_parameter('publish_topic', '/lidar/imu_covariance').value + self.declare_parameter('subscribe_topic', '/lidar/imu').value + self.declare_parameter('linear_acceleration_covariance', [0.0] * 9) + self.declare_parameter('angular_velocity_covariance', [0.0] * 9) + self.declare_parameter('orientation_covariance', [0.0] * 9) + + self.orientation_covariance = self.get_parameter('orientation_covariance').value + self.linear_acceleration_covariance = self.get_parameter('linear_acceleration_covariance').value + self.angular_velocity_covariance = self.get_parameter('angular_velocity_covariance').value + self.publish_topic = self.get_parameter('publish_topic').value + self.subscribe_topic = self.get_parameter('subscribe_topic').value + + self.publisher = self.create_publisher(Imu, self.publish_topic, 10) + + + self.subscription = self.create_subscription( + Imu, + self.subscribe_topic, + self.imu_callback, + 10) + + def imu_callback(self, msg): + # Update covariances in-place + msg.orientation_covariance = self.orientation_covariance + msg.linear_acceleration_covariance = self.linear_acceleration_covariance + msg.angular_velocity_covariance = self.angular_velocity_covariance + + self.publisher.publish(msg) + +def main(args=None): + rclpy.init(args=args) + + imu_covariance_node = ImuCovarianceNode() + + rclpy.spin(imu_covariance_node) + + imu_covariance_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main()