t.korjakow/ft24_control
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Switched to ROS Iron

Browse Source
This commit is contained in:
wittenator 2023-11-04 11:41:43 +01:00
parent 96c69e859c
commit d7857512f1
5 changed files with 42 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/dcaiti_control/config/dcaiti_config.yml
View File

@ -15,6 +15,7 @@ ackermann_steering_controller:
base_frame_id: base_link base_frame_id: base_link
publish_limited_velocity: true publish_limited_velocity: true
in_chained_mode: false in_chained_mode: false
enable_odom_tf: false
reference_timeout: 2.0 reference_timeout: 2.0
front_steering: true front_steering: true

49
src/dcaiti_control/config/ukf.yml
View File

@ -15,7 +15,7 @@ ukf_filter_node:
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar # 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 # 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. # by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: True two_d_mode: true
# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for # 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 # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
@ -63,7 +63,6 @@ ukf_filter_node:
# 3a. Set your "world_frame" to your map_frame value # 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 # 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. # 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 odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" 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 world_frame: odom # Defaults to the value of odom_frame if unspecified
@ -117,22 +116,22 @@ ukf_filter_node:
odom0_pose_rejection_threshold: 5.0 odom0_pose_rejection_threshold: 5.0
odom0_twist_rejection_threshold: 1.0 odom0_twist_rejection_threshold: 1.0
#imu0: lidar/imu_covariance imu0: lidar/imu_covariance
#imu0_config: [false, false, false, imu0_config: [false, false, false,
# true, true, true, true, true, true,
# false, false, false, false, false, false,
# true, true, true, true, true, true,
# true, true, true] true, true, true]
#imu0_differential: false imu0_differential: false
#imu0_relative: true imu0_relative: true
#imu0_queue_size: 5 imu0_queue_size: 5
#imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
#imu0_twist_rejection_threshold: 0.8 # imu0_twist_rejection_threshold: 0.8 #
#imu0_linear_acceleration_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 # [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. # 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 imu0_remove_gravitational_acceleration: true
# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no # [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 # acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
@ -182,28 +181,13 @@ ukf_filter_node:
# unspecified. # unspecified.
# Note: the specification of covariance matrices can be cumbersome, so all matrix parameters in this package support # 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. # 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 # [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 # 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 # 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 # 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. # 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 # [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. # with UKFs, it's probably a good idea to leave these alone.
@ -214,4 +198,5 @@ ukf_filter_node:
# [ADVANCED, UKF ONLY] The beta variable relates to the distribution of the state vector. Again, it's probably best to # [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. # leave this alone if you're uncertain. Defaults to 2 if unspecified.
beta: 2.0 beta: 2.0
use_sime_time: true

12
src/dcaiti_control/description/ros2_control.xacro
View File

@ -2,7 +2,7 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" > <robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
<ros2_control name="GazeboSimSystem" type="system"> <ros2_control name="GazeboSimSystem" type="system">
<hardware> <hardware>
<plugin>ign_ros2_control/IgnitionSystem</plugin> <plugin>gz_ros2_control/GazeboSimSystem</plugin>
</hardware> </hardware>
<joint name="rear_right_wheel_joint"> <joint name="rear_right_wheel_joint">
<command_interface name="velocity"/> <command_interface name="velocity"/>
@ -26,9 +26,9 @@
</joint> </joint>
</ros2_control> </ros2_control>
<gazebo> <gazebo>
<plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin"> <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system">
<parameters>$(find dcaiti_control)/config/dcaiti_config.yml</parameters> <parameters>$(find dcaiti_control)/config/dcaiti_config.yml</parameters>
<parameters>$(find dcaiti_control)/config/gaz_ros2_ctl_sim.yml</parameters> <parameters>$(find dcaiti_control)/config/gaz_ros2_ctl_sim.yml</parameters>
</plugin> </plugin>
</gazebo> </gazebo>
</robot> </robot>

22
src/dcaiti_control/launch/launch_sim.py
View File

@ -101,22 +101,22 @@ def generate_launch_description():
name='imu_covariance_adder', name='imu_covariance_adder',
output='screen', output='screen',
parameters=[ parameters=[
{'orientation_covariance': [1e-3]*9}, {'orientation_covariance': [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]},
{'linear_acceleration_covariance': [1e-3]*9}, {'linear_acceleration_covariance': [0.0002, 0.0, 0.0, 0.0, 0.0002, 0.0, 0.0, 0.0, 0.0002]},
{'angular_velocity_covariance': [1e-3]*9}, {'angular_velocity_covariance': [0.0001, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0001]},
{'subscribe_topic': '/lidar/imu'}, {'subscribe_topic': '/lidar/imu'},
{'publish_topic': '/lidar/imu_covariance'} {'publish_topic': '/lidar/imu_covariance'}
] ]
) )
#ukf_node = Node( ukf_node = Node(
# package='robot_localization', package='robot_localization',
# executable='ukf_node', executable='ukf_node',
# name='ukf_filter_node', name='ukf_filter_node',
# output='screen', output='screen',
# parameters=[str(base_path / 'config' / 'ukf.yml')], parameters=[str(base_path / 'config' / 'ukf.yml')],
# remappings=[('odometry/filtered', 'odometry/local')] remappings=[('odometry/filtered', 'odometry/local')]
# ) )
# Launch them all! # Launch them all!

14
src/dcaiti_control/worlds/empty.sdf.template
View File

@ -19,25 +19,25 @@
</dart> </dart>
</physics> </physics>
<plugin <plugin
filename="ignition-gazebo-physics-system" filename="gz-sim-physics-system"
name="gz::sim::systems::Physics"> name="gz::sim::systems::Physics">
</plugin> </plugin>
<plugin <plugin
filename="ignition-gazebo-user-commands-system" filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands"> name="gz::sim::systems::UserCommands">
</plugin> </plugin>
<plugin <plugin
filename="ignition-gazebo-scene-broadcaster-system" filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster"> name="gz::sim::systems::SceneBroadcaster">
</plugin> </plugin>
<plugin <plugin
filename="ignition-gazebo-contact-system" filename="gz-sim-contact-system"
name="gz::sim::systems::Contact"> name="gz::sim::systems::Contact">
</plugin> </plugin>
<plugin filename="libignition-gazebo-imu-system.so" <plugin filename="gz-sim-imu-system.so"
name="ignition::gazebo::systems::Imu"> name="gz::sim::systems::Imu">
</plugin> </plugin>
<plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors"> <plugin filename="gz-sim-sensors-system.so" name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine> <render_engine>ogre2</render_engine>
</plugin> </plugin>
<scene> <scene>