Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fuse odometry data using robot localization #268

Open
wants to merge 1 commit into
base: comp-2024
Choose a base branch
from
Open

Fuse odometry data using robot localization #268

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion ...rs_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/costmap_parameters.yaml
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading