Skip to content

Commit

Permalink
fuse odometry data using robot localization
Browse files Browse the repository at this point in the history
  • Loading branch information
@nico-palmar
nico-palmar committed May 19, 2024
1 parent 1a3d359 commit 17c4957
Show file tree
Hide file tree
Showing 7 changed files with 265 additions and 6 deletions.
3 changes: 2 additions & 1 deletion ...rs_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/costmap_parameters.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -176,7 +176,8 @@ map_saver:

map_server:
ros__parameters:
yaml_filename: "/home/meshva/uwrt/uwrt_mars_rover/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/map.yaml"
# TODO: Change name "nico" to use the USER env var instead
yaml_filename: "/home/nico/uwrt_ws/src/uwrt_mars_rover/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/map.yaml"
topic_name: "map"
frame_id: "odom"

Expand Down
51 changes: 51 additions & 0 deletions ...over_drivetrain/uwrt_mars_rover_drivetrain_description/config/dual_ekf_navsat_params.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
ekf_filter_node_odom:
ros__parameters:
frequency: 30.0
two_d_mode: true # Recommended to use 2d mode for nav2 in mostly planar environments
print_diagnostics: true
debug: false
publish_tf: true

# map_frame: odom
odom_frame: odom
base_link_frame: base_link # the frame id used by the diff drive plugin
world_frame: odom

odom0: odom
odom0_config: [false, false, false, # TODO: test switching this row to all true
false, false, false,
true, true, true, # start with the wheel velocities; try switching to positions later and see what works better
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_differential: false
odom0_relative: false

imu0: imu/data
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, false,
false, false, false]
imu0_differential: false # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true

use_control: false

process_noise_covariance: [1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.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.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3]
32 changes: 32 additions & 0 deletions ..._rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/dual_ekf_navsat.launch.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
import launch_ros.actions
import os
import launch.actions


def generate_launch_description():
gps_wpf_dir = os.path.join(get_package_share_directory('uwrt_mars_rover_drivetrain_description'))
rl_params_file = os.path.join(
gps_wpf_dir, "config", "dual_ekf_navsat_params.yaml")

return LaunchDescription(
[
launch.actions.DeclareLaunchArgument(
"output_final_position", default_value="false"
),
launch.actions.DeclareLaunchArgument(
"output_location", default_value="~/dual_ekf_navsat_example_debug.txt"
),
launch_ros.actions.Node(
package="robot_localization",
executable="ekf_node",
name="ekf_filter_node_odom",
output="screen",
parameters=[rl_params_file, {"use_sim_time": True}],
remappings=[("odometry/filtered", "odometry/local")],
),
]
)
14 changes: 12 additions & 2 deletions uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/nav.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,14 @@

def generate_launch_description():

declared_arguments = []
declared_arguments = [
# TODO: add this back after fixing the map path with the USER env var
# DeclareLaunchArgument(
# "map_yaml_filename",
# default_value=os.path.join(get_package_share_directory('uwrt_mars_rover_drivetrain_description'), 'config', 'map.yaml'),
# description='Map yaml file path'
# )
]
nodes = []

controller_yaml = os.path.join(get_package_share_directory('uwrt_mars_rover_drivetrain_description'), 'config', 'costmap_parameters.yaml')
Expand All @@ -23,7 +30,10 @@ def generate_launch_description():
package='nav2_map_server',
executable='map_server',
output='screen',
parameters=[controller_yaml])]
# TODO: add this back after fixing the map path with the USER env var
# parameters=[{"yaml_file": LaunchConfiguration("map_yaml_filename")}, controller_yaml],
parameters=[controller_yaml]
)]

nodes += [Node(
package='nav2_controller',
Expand Down
164 changes: 163 additions & 1 deletion ..._mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.macro.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
ixx="1e-3" ixy="0.0" ixz="0.0"
iyy="1e-3" iyz="0.0"
izz="1e-3"/>
</inertial>
</inertial>
</xacro:macro>

<xacro:macro name="chassis_inertia" params="chassis_width:=^ chassis_height:=^ chassis_length:=^ chassis_mass:=^">
Expand Down Expand Up @@ -134,4 +134,166 @@
</ros2_control>
</xacro:macro>

<xacro:macro name="nav_imu">
<link name="nav_imu_link">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
<!-- <material name="cyan"> -->
<!-- <color rgba"0 1.0 1.0 1.0"/> -->
<!-- </material> -->
</visual>

<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<xacro:default_inertial mass="0.001"/>
</link>

<gazebo reference="nav_imu_link">
<sensor name="nav_imu_sensor" type="imu">
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<ros>
<remapping>~/out:=imu/data</remapping>
</ros>
<initial_orientation_as_reference>false</initial_orientation_as_reference>
</plugin>
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<!-- <bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev> -->
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<!-- <bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev> -->
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<!-- <bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev> -->
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<!-- <bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev> -->
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<!-- <bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev> -->
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<!-- <bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev> -->
</noise>
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>

<joint name="nav_imu_joint" type="fixed">
<parent link="base_link"/>
<child link="nav_imu_link"/>
<origin xyz ="0 0 0.1"/>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>

</xacro:macro>

<xacro:macro name="nav_gps">

<link name="gps_link">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
<!-- <material name="cyan"> -->
<!-- <color rgba"0 1.0 1.0 1.0"/> -->
<!-- </material> -->
</visual>

<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<xacro:default_inertial mass="0.001"/>
</link>

<gazebo reference="gps_link">
<sensor name="gps_sensor" type="gps">
<plugin name="gps_plugin" filename="libgazebo_ros_gps_sensor.so">
<ros>
<remapping>~/out:=gps/fix</remapping>
</ros>
<frame_name>gps_link</frame_name>
</plugin>
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<gps>

<longitude>
<mean>0.0</mean>
<stddev>0.01</stddev>
<!-- <bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev> -->
</longitude>

<latitude>
<mean>0.0</mean>
<stddev>0.01</stddev>
<!-- <bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev> -->
</latitude>

</gps>
</sensor>
<material>Gazebo/Grey</material> <!-- TODO: fix? -->
</gazebo>

<joint name="gps_joint" type="fixed">
<parent link="base_link"/>
<child link="gps_link"/>
<origin xyz="0 0 0.1"/>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>

</xacro:macro>


</robot>
5 changes: 4 additions & 1 deletion uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,10 @@
</joint>



<!-- sensors -->
<xacro:nav_imu/>
<xacro:nav_gps/>

<!-- wheels -->
<xacro:wheel prefix="right" suffix="front" parent_link="chassis" reflect="-1" x_position="${chassis_length/2}"/>
<xacro:wheel prefix="right" suffix="middle" parent_link="chassis" reflect="-1" x_position="0"/>
Expand Down
2 changes: 1 addition & 1 deletion uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/config/drivetrain_controllers.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ differential_drivetrain_controller:
twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.01 ]

publish_rate: 50.0
enable_odom_tf: true
enable_odom_tf: false
open_loop: false # If true, integrates vel_cmd to estimate odom, else uses wheel feedback to compute odom
position_feedback: true # If true, uses position state_interface for odom calculations, else use velocity state interface
# TODO: this setting doesnt really make sense to me to tie both odom pose and twist calculations to either use position or
Expand Down

0 comments on commit 17c4957

Please sign in to comment.