Tried to fix ukf
This commit is contained in:
wittenator
parent
60a798f5d2
commit
96c69e859c
@ -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()
|
||||
|
||||
217
src/dcaiti_control/config/ukf.yml
Normal file
217
src/dcaiti_control/config/ukf.yml
Normal file
@ -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<double>::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
|
||||
0
src/dcaiti_control/dcaiti_control/__init__.py
Normal file
0
src/dcaiti_control/dcaiti_control/__init__.py
Normal file
@ -3,10 +3,57 @@
|
||||
<gazebo reference="laser_frame">
|
||||
<material>Gazebo/Red</material>
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<ignition_frame_id>laser_frame</ignition_frame_id>
|
||||
<always_on>1</always_on>
|
||||
<update_rate>50</update_rate>
|
||||
<visualize>false</visualize>
|
||||
<topic>/lidar/imu</topic>
|
||||
<angular_velocity>
|
||||
<x>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</x>
|
||||
<y>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</y>
|
||||
<z>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</z>
|
||||
</angular_velocity>
|
||||
<linear_acceleration>
|
||||
<x>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</x>
|
||||
<y>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</y>
|
||||
<z>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</z>
|
||||
</linear_acceleration>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
</robot>
|
||||
@ -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.
|
||||
@ -95,6 +95,29 @@ def generate_launch_description():
|
||||
)
|
||||
)
|
||||
|
||||
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!
|
||||
return LaunchDescription([
|
||||
@ -112,4 +135,6 @@ def generate_launch_description():
|
||||
spawn_entity,
|
||||
delayed_diff_drive_spawner,
|
||||
delayed_joint_broad_spawner,
|
||||
imu_covariance_adder,
|
||||
#ukf_node
|
||||
])
|
||||
|
||||
@ -67,5 +67,5 @@ def generate_launch_description():
|
||||
|
||||
node_robot_state_publisher,
|
||||
node_joint_state_publisher,
|
||||
twist_mux,
|
||||
#twist_mux,
|
||||
])
|
||||
|
||||
0
src/dcaiti_control/src/__init__.py
Normal file
0
src/dcaiti_control/src/__init__.py
Normal file
51
src/dcaiti_control/src/imu_covariance_adder.py
Executable file
51
src/dcaiti_control/src/imu_covariance_adder.py
Executable file
@ -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()
|
||||
Loading…
x
Reference in New Issue
Block a user