diff --git a/.DS_Store b/.DS_Store
new file mode 100644
index 00000000..917123cc
Binary files /dev/null and b/.DS_Store differ
diff --git a/urc_arm/model/meshes/collision/leftgripper_collision.STL b/urc_arm/model/meshes/collision/leftgripper_collision.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/collision/rightgripper_collision.STL b/urc_arm/model/meshes/collision/rightgripper_collision.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/base_link.STL b/urc_arm/model/meshes/old/base_link.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/base_link_collision.STL b/urc_arm/model/meshes/old/base_link_collision.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/bicep.STL b/urc_arm/model/meshes/old/bicep.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/bicep_collision.STL b/urc_arm/model/meshes/old/bicep_collision.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/claw.STL b/urc_arm/model/meshes/old/claw.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/claw_collision.STL b/urc_arm/model/meshes/old/claw_collision.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/elbow.STL b/urc_arm/model/meshes/old/elbow.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/elbow_collision.STL b/urc_arm/model/meshes/old/elbow_collision.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/forearm.STL b/urc_arm/model/meshes/old/forearm.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/forearm_collision.STL b/urc_arm/model/meshes/old/forearm_collision.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/leftgripper.STL b/urc_arm/model/meshes/old/leftgripper.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/leftgripper_collision.STL b/urc_arm/model/meshes/old/leftgripper_collision.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/rightgripper.STL b/urc_arm/model/meshes/old/rightgripper.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/rightgripper_collision.STL b/urc_arm/model/meshes/old/rightgripper_collision.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/shoulder.STL b/urc_arm/model/meshes/old/shoulder.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/shoulder_collision.STL b/urc_arm/model/meshes/old/shoulder_collision.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/wrist.STL b/urc_arm/model/meshes/old/wrist.STL
old mode 100755
new mode 100644
diff --git a/urc_arm/model/meshes/old/wrist_collision.STL b/urc_arm/model/meshes/old/wrist_collision.STL
old mode 100755
new mode 100644
diff --git a/urc_bringup/.DS_Store b/urc_bringup/.DS_Store
new file mode 100644
index 00000000..5008ddfc
Binary files /dev/null and b/urc_bringup/.DS_Store differ
diff --git a/urc_bringup/launch/bringup_simulation.launch.py b/urc_bringup/launch/bringup_simulation.launch.py
index 5a7807ac..cbd0f134 100644
--- a/urc_bringup/launch/bringup_simulation.launch.py
+++ b/urc_bringup/launch/bringup_simulation.launch.py
@@ -7,7 +7,7 @@
RegisterEventHandler,
)
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch.event_handlers import OnProcessExit, OnProcessStart
+from launch.event_handlers import OnProcessStart, OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
@@ -20,6 +20,7 @@ def generate_launch_description():
pkg_urc_bringup = get_package_share_directory("urc_bringup")
pkg_path_planning = get_package_share_directory("path_planning")
pkg_trajectory_following = get_package_share_directory("trajectory_following")
+ pkg_urc_localization = get_package_share_directory("urc_localization")
pkg_urc_test = get_package_share_directory("urc_test")
controller_config_file_dir = os.path.join(
@@ -153,6 +154,16 @@ def generate_launch_description():
)
)
+ ekf_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(
+ pkg_urc_localization,
+ "launch",
+ "ekf.launch.py"
+ )
+ )
+ )
+
elevation_mapping_node = Node(
package="urc_perception",
executable="urc_perception_ElevationMapping",
@@ -168,22 +179,6 @@ def generate_launch_description():
],
)
- map_to_odom_transform_node = Node(
- package="tf2_ros",
- executable="static_transform_publisher",
- arguments=["10", "10", "0", "0", "0", "0", "map", "odom"],
- )
-
- map_to_odom_launch = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- os.path.join(
- pkg_urc_test,
- "launch",
- "odom_to_map_pose.launch.py",
- )
- )
- )
-
return LaunchDescription(
[
RegisterEventHandler(
@@ -193,7 +188,6 @@ def generate_launch_description():
load_joint_state_broadcaster,
load_drivetrain_controller,
teleop_launch,
- map_to_odom_launch,
],
)
),
@@ -217,6 +211,6 @@ def generate_launch_description():
gazebo,
load_robot_state_publisher,
spawn_robot,
- map_to_odom_transform_node,
+ ekf_launch
]
)
diff --git a/urc_hw_description/.setup_assistant b/urc_hw_description/.setup_assistant
old mode 100755
new mode 100644
diff --git a/urc_hw_description/config/initial_positions.yaml b/urc_hw_description/config/initial_positions.yaml
old mode 100755
new mode 100644
diff --git a/urc_hw_description/config/joint_limits.yaml b/urc_hw_description/config/joint_limits.yaml
old mode 100755
new mode 100644
diff --git a/urc_hw_description/config/kinematics.yaml b/urc_hw_description/config/kinematics.yaml
old mode 100755
new mode 100644
diff --git a/urc_hw_description/config/moveit.rviz b/urc_hw_description/config/moveit.rviz
old mode 100755
new mode 100644
diff --git a/urc_hw_description/config/moveit_controllers.yaml b/urc_hw_description/config/moveit_controllers.yaml
old mode 100755
new mode 100644
diff --git a/urc_hw_description/config/pilz_cartesian_limits.yaml b/urc_hw_description/config/pilz_cartesian_limits.yaml
old mode 100755
new mode 100644
diff --git a/urc_hw_description/config/ros2_controllers.yaml b/urc_hw_description/config/ros2_controllers.yaml
old mode 100755
new mode 100644
diff --git a/urc_hw_description/urdf/walli_sensors.xacro b/urc_hw_description/urdf/walli_sensors.xacro
index 8a9ebeca..2a655b29 100644
--- a/urc_hw_description/urdf/walli_sensors.xacro
+++ b/urc_hw_description/urdf/walli_sensors.xacro
@@ -38,24 +38,24 @@
0.0
2e-4
- 0.0000075
- 0.0000008
+ 0.0
+ 0.0
0.0
2e-4
- 0.0000075
- 0.0000008
+ 0.0
+ 0.0
0.0
2e-4
- 0.0000075
- 0.0000008
+ 0.0
+ 0.0
@@ -64,24 +64,24 @@
0.0
1.7e-2
- 0.1
- 0.001
+ 0.0
+ 0.0
0.0
1.7e-2
- 0.1
- 0.001
+ 0.0
+ 0.0
0.0
1.7e-2
- 0.1
- 0.001
+ 0.0
+ 0.0
@@ -116,7 +116,7 @@
/gps
~/out:=data
- gps
+ map
true
30
@@ -347,4 +347,4 @@
0.00
-
\ No newline at end of file
+
diff --git a/urc_localization/.DS_Store b/urc_localization/.DS_Store
new file mode 100644
index 00000000..5008ddfc
Binary files /dev/null and b/urc_localization/.DS_Store differ
diff --git a/urc_localization/config/dual_ekf_navsat_example.yaml b/urc_localization/config/dual_ekf_navsat_example.yaml
deleted file mode 100644
index 488ce55f..00000000
--- a/urc_localization/config/dual_ekf_navsat_example.yaml
+++ /dev/null
@@ -1,107 +0,0 @@
-# For parameter descriptions, please refer to the template parameter files for each node.
-
-ekf_filter_node_odom:
- ros__parameters:
- frequency: 30.0
- sensor_timeout: 0.1
- two_d_mode: false
- transform_time_offset: 0.0
- transform_timeout: 0.0
- print_diagnostics: true
- debug: false
-
- map_frame: map
- odom_frame: odom
- base_link_frame: base_link
- world_frame: odom
-
- odom0: odometry/wheel
- odom0_config: [false, false, false,
- false, false, false,
- true, true, true,
- false, false, true,
- false, false, false]
- odom0_queue_size: 10
- odom0_differential: false
- odom0_relative: false
-
- imu0: imu/data
- imu0_config: [false, false, false,
- true, true, false,
- false, false, false,
- true, true, true,
- true, true, true]
- imu0_differential: false
- imu0_relative: false
- imu0_queue_size: 10
- imu0_remove_gravitational_acceleration: true
-
- use_control: false
-
- process_noise_covariance: [1e-3, 1e-3, 1e-3, 0.3, 0.3, 0.01, 0.5, 0.5, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3]
-
- initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1.0, 1.0, 1e-9, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
-
-ekf_filter_node_map:
- ros__parameters:
- frequency: 30.0
- sensor_timeout: 0.1
- two_d_mode: false
- transform_time_offset: 0.0
- transform_timeout: 0.0
- print_diagnostics: true
- debug: false
-
- map_frame: map
- odom_frame: odom
- base_link_frame: base_link
- world_frame: map
-
- odom0: odometry/wheel
- odom0_config: [false, false, false,
- false, false, false,
- true, true, true,
- false, false, true,
- false, false, false]
- odom0_queue_size: 10
- odom0_differential: false
- odom0_relative: false
-
- odom1: odometry/gps
- odom1_config: [true, true, false,
- false, false, false,
- false, false, false,
- false, false, false,
- false, false, false]
- odom1_queue_size: 10
- odom1_differential: false
- odom1_relative: false
-
- imu0: imu/data
- imu0_config: [false, false, false,
- true, true, false,
- false, false, false,
- true, true, true,
- true, true, true]
- imu0_differential: false
- imu0_relative: false
- imu0_queue_size: 10
- imu0_remove_gravitational_acceleration: true
-
- use_control: false
-
- process_noise_covariance: [1.0, 1.0, 1e-3, 0.3, 0.3, 0.01, 0.5, 0.5, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3]
-
- initial_estimate_covariance: [1.0, 1.0, 1e-9, 1.0, 1.0, 1e-9, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
-
-navsat_transform:
- ros__parameters:
- frequency: 30.0
- delay: 3.0
- magnetic_declination_radians: 0.0429351 # For lat/long 55.944831, -3.186998
- yaw_offset: 1.570796327 # IMU reads 0 facing magnetic north, not east
- zero_altitude: false
- broadcast_utm_transform: true
- publish_filtered_gps: true
- use_odometry_yaw: false
- wait_for_datum: false
diff --git a/urc_localization/config/dual_ekf_navsat_params.yaml b/urc_localization/config/dual_ekf_navsat_params.yaml
deleted file mode 100644
index b14b9a34..00000000
--- a/urc_localization/config/dual_ekf_navsat_params.yaml
+++ /dev/null
@@ -1,238 +0,0 @@
-# For parameter descriptions, please refer to the template parameter files for each node.
-
-ekf_filter_node_odom:
- ros__parameters:
- frequency: 30.0
- sensor_timeout: 0.1
- two_d_mode: false
- transform_time_offset: 0.0
- transform_timeout: 0.0
- print_diagnostics: true
- debug: false
-
- odom_frame: odom
- base_link_frame: base_link
- world_frame: odom
-
- odom0: /rover_drivetrain_controller/odom
- odom0_config:
- [
- false,
- false,
- false,
- false,
- false,
- false,
- true,
- true,
- true,
- false,
- false,
- true,
- false,
- false,
- false,
- ]
- odom0_queue_size: 10
- odom0_differential: false
- odom0_relative: false
-
- imu0: /imu/data
- imu0_config:
- [
- false,
- false,
- false,
- true,
- true,
- false,
- false,
- false,
- false,
- true,
- true,
- true,
- true,
- true,
- true,
- ]
- imu0_differential: false
- imu0_relative: false
- imu0_queue_size: 10
- imu0_remove_gravitational_acceleration: true
-
- use_control: false
-
- process_noise_covariance:
- [
- 1e-3,
- 1e-3,
- 1e-3,
- 0.3,
- 0.3,
- 0.01,
- 0.5,
- 0.5,
- 0.1,
- 0.3,
- 0.3,
- 0.3,
- 0.3,
- 0.3,
- 0.3,
- ]
-
- initial_estimate_covariance:
- [
- 1e-9,
- 1e-9,
- 1e-9,
- 1.0,
- 1.0,
- 1e-9,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- ]
-
-ekf_filter_node_map:
- ros__parameters:
- frequency: 30.0
- sensor_timeout: 0.1
- two_d_mode: false
- transform_time_offset: 0.0
- transform_timeout: 0.0
- print_diagnostics: true
- debug: false
-
- odom_frame: odom
- base_link_frame: base_link
- world_frame: odom
-
- odom0: /rover_drivetrain_controller/odom
- odom0_config:
- [
- false,
- false,
- false,
- false,
- false,
- false,
- true,
- true,
- true,
- false,
- false,
- true,
- false,
- false,
- false,
- ]
- odom0_queue_size: 10
- odom0_differential: false
- odom0_relative: false
-
- odom1: odometry/gps
- odom1_config:
- [
- true,
- true,
- false,
- false,
- false,
- false,
- false,
- false,
- false,
- false,
- false,
- false,
- false,
- false,
- false,
- ]
- odom1_queue_size: 10
- odom1_differential: false
- odom1_relative: false
-
- imu0: imu/data
- imu0_config:
- [
- false,
- false,
- false,
- true,
- true,
- false,
- false,
- false,
- false,
- true,
- true,
- true,
- true,
- true,
- true,
- ]
- imu0_differential: false
- imu0_relative: false
- imu0_queue_size: 10
- imu0_remove_gravitational_acceleration: true
-
- use_control: false
-
- process_noise_covariance:
- [
- 1.0,
- 1.0,
- 1e-3,
- 0.3,
- 0.3,
- 0.01,
- 0.5,
- 0.5,
- 0.1,
- 0.3,
- 0.3,
- 0.3,
- 0.3,
- 0.3,
- 0.3,
- ]
-
- initial_estimate_covariance:
- [
- 1.0,
- 1.0,
- 1e-9,
- 1.0,
- 1.0,
- 1e-9,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- 1.0,
- ]
-
-navsat_transform:
- ros__parameters:
- frequency: 30.0
- delay: 3.0
- magnetic_declination_radians: 0.0 # For lat/long 55.944831, -3.186998
- yaw_offset: 3.14159265359 # IMU reads 0 facing magnetic north, not east
- zero_altitude: false
- broadcast_utm_transform: false
- publish_filtered_gps: false
- use_odometry_yaw: false
- wait_for_datum: false
diff --git a/urc_localization/config/ekf.yaml b/urc_localization/config/ekf.yaml
new file mode 100644
index 00000000..d8e5fc3e
--- /dev/null
+++ b/urc_localization/config/ekf.yaml
@@ -0,0 +1,182 @@
+ekf_filter_node_odom:
+ ros__parameters:
+ frequency: 30.0
+ sensor_timeout: 5.0
+ two_d_mode: true
+ transform_time_offset: 0.0
+ transform_timeout: 0.0
+ print_diagnostics: true
+ debug: false
+
+ map_frame: map
+ odom_frame: odom
+ base_link_frame: base_link
+ world_frame: odom
+
+ imu0: /imu/data
+ imu0_config: [false, false, false,
+ false, false, true,
+ false, false, false,
+ false, false, true,
+ true, true, false]
+ imu0_differential: false
+ imu0_relative: false
+ imu0_queue_size: 10
+ imu_remove_gravitational_acceleration: false
+
+ odom0: /rover_drivetrain_controller/odom
+ odom0_config: [false, false, false,
+ false, false, true,
+ true, true, false,
+ false, false, false,
+ false, false, false]
+ odom0_differential: false
+ odom0_relative: true
+ odom0_queue_size: 10
+
+ use_control: false
+ # 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]
+ #
+ #
+ # initial_estimate_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]
+ dynamic_process_noise_covariance: true
+ print_diagnostics: true
+
+
+ekf_filter_node_map:
+ ros__parameters:
+ #Frequency the filter outputs a position estimate
+ frequency: 30.0
+ #Period in seconds after which we consider a sensor to have timed out
+ sensor_timeout: 5.0
+
+ # 2D vs 3D data
+ two_d_mode: false
+
+ #Transform time offset
+ transform_time_offset: 0.0
+
+ #How long tf-listener waits for transform to become available
+ transform_timeout: 0.0
+
+ publish_acceleration: true
+
+ #Publish transformation over tf topic
+ publish_tf: true
+
+
+ map_frame: map
+ odom_frame: odom
+ base_link_frame: base_link
+ world_frame: map
+
+
+ odom0: /odometry/gps
+ odom0_config: [true, true, false,
+ false, false, false,
+ false, false, false,
+ false, false, false,
+ false, false, false]
+ odom0_queue_size: 10
+ odom0_differential: false
+ odom0_relative: false
+
+ odom1: /rover_drivetrain_controller/odom
+ odom1_config: [true, true, false,
+ false, false, true,
+ true, true, false,
+ false, false, false,
+ false, false, false]
+ odom1_differential: false
+ odom1_relative: true
+ odom1_queue_size: 10
+
+ imu0: /imu/data
+ imu0_config: [true, true, false,
+ false, false, true,
+ true, true, false,
+ false, false, true,
+ true, true, false]
+ imu0_differential: true
+ imu0_relative: false
+ imu0_queue_size: 10
+ imu_remove_gravitational_acceleration: false
+
+ use_control: false
+ # 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]
+ # initial_estimate_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]
+ #
+ dynamic_process_noise_covariance: true
+
+
+
+navsat_transform:
+ ros__parameters:
+ # Frequency of the main run loop
+ frequency: 30.0
+ # Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame.
+ delay: 2.0
+ magnetic_declination_radians: -0.06736971
+ yaw_offset: 0.0
+ zero_altitude: true
+ broadcast_utm_transform: true
+ publish_filtered_gps: true
+ use_odometry_yaw: true
+ wait_for_datum: false
+
diff --git a/urc_localization/launch/dual_ekf_navsat.launch.py b/urc_localization/launch/dual_ekf_navsat.launch.py
index 83e3408a..555636be 100644
--- a/urc_localization/launch/dual_ekf_navsat.launch.py
+++ b/urc_localization/launch/dual_ekf_navsat.launch.py
@@ -1,66 +1,66 @@
-# Copyright 2018 Open Source Robotics Foundation, Inc.
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
+# # Copyright 2018 Open Source Robotics Foundation, Inc.
+# # Licensed under the Apache License, Version 2.0 (the "License");
+# # you may not use this file except in compliance with the License.
+# # You may obtain a copy of the License at
+# #
+# # http://www.apache.org/licenses/LICENSE-2.0
+# #
+# # Unless required by applicable law or agreed to in writing, software
+# # distributed under the License is distributed on an "AS IS" BASIS,
+# # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# # See the License for the specific language governing permissions and
+# # limitations under the License.
-from launch import LaunchDescription
-from ament_index_python.packages import get_package_share_directory
-import launch_ros.actions
-import os
-import launch.actions
+# from launch import LaunchDescription
+# from ament_index_python.packages import get_package_share_directory
+# import launch_ros.actions
+# import os
+# import launch.actions
-def generate_launch_description():
- gps_wpf_dir = get_package_share_directory("urc_localization")
- rl_params_file = os.path.join(gps_wpf_dir, "config", "dual_ekf_navsat_params.yaml")
+# def generate_launch_description():
+# gps_wpf_dir = get_package_share_directory("urc_localization")
+# 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")],
- # ),
- # launch_ros.actions.Node(
- # package="robot_localization",
- # executable="ekf_node",
- # name="ekf_filter_node_map",
- # output="screen",
- # parameters=[rl_params_file, {"use_sim_time": True}],
- # remappings=[("odometry/filtered", "odometry/global")],
- # ),
- launch_ros.actions.Node(
- package="robot_localization",
- executable="navsat_transform_node",
- name="navsat_transform",
- output="screen",
- parameters=[rl_params_file, {"use_sim_time": True}],
- remappings=[
- # Subscribes to
- ("imu", "imu/data"),
- ("gps/fix", "gps/data"),
- ("odometry/filtered", "rover_drivetrain_controller/odom"),
- # Publishes to
- ("gps/filtered", "gps/filtered"),
- ("odometry/gps", "odometry/gps"),
- ],
- ),
- ]
- )
+# 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")],
+# # ),
+# # launch_ros.actions.Node(
+# # package="robot_localization",
+# # executable="ekf_node",
+# # name="ekf_filter_node_map",
+# # output="screen",
+# # parameters=[rl_params_file, {"use_sim_time": True}],
+# # remappings=[("odometry/filtered", "odometry/global")],
+# # ),
+# launch_ros.actions.Node(
+# package="robot_localization",
+# executable="navsat_transform_node",
+# name="navsat_transform",
+# output="screen",
+# parameters=[rl_params_file, {"use_sim_time": True}],
+# remappings=[
+# # Subscribes to
+# ("imu", "imu/data"),
+# ("gps/fix", "gps/data"),
+# ("odometry/filtered", "rover_drivetrain_controller/odom"),
+# # Publishes to
+# ("gps/filtered", "gps/filtered"),
+# ("odometry/gps", "odometry/gps"),
+# ],
+# ),
+# ]`
+# )
diff --git a/urc_localization/launch/ekf.launch.py b/urc_localization/launch/ekf.launch.py
new file mode 100644
index 00000000..eb32def7
--- /dev/null
+++ b/urc_localization/launch/ekf.launch.py
@@ -0,0 +1,84 @@
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+import launch_ros.actions
+import os
+from launch.actions import (
+ RegisterEventHandler
+)
+from launch.event_handlers import OnProcessStart
+
+
+def generate_launch_description():
+
+ ekf_global = launch_ros.actions.Node(
+ package="robot_localization",
+ executable="ekf_node",
+ name="ekf_filter_node_map",
+ output="screen",
+ parameters=[
+ os.path.join(
+ get_package_share_directory("urc_localization"),
+ "config",
+ "ekf.yaml",
+ )
+ ],
+ remappings=[
+ ("/odometry/filtered","/odometry/filtered_global")
+ ],
+ )
+
+ ekf_local = launch_ros.actions.Node(
+ package="robot_localization",
+ executable="ekf_node",
+ name="ekf_filter_node_odom",
+ output="screen",
+ parameters=[
+ os.path.join(
+ get_package_share_directory("urc_localization"),
+ "config",
+ "ekf.yaml",
+ )
+ ],
+ remappings=[
+
+ ("/odometry/filtered","odometry/filtered_local")
+ ],
+ )
+
+ navsat = launch_ros.actions.Node(
+ package="robot_localization",
+ executable="navsat_transform_node",
+ name="navsat_transform",
+ output="screen",
+ parameters=[
+ os.path.join(
+ get_package_share_directory("urc_localization"),
+ "config",
+ "ekf.yaml",
+ )
+ ],
+ remappings=[
+ ("/imu","/imu/data"),
+ ("/odometry/filtered","/odometry/filtered_global"),
+ ("/gps/fix","/gps/data")
+ ]
+ )
+
+ return LaunchDescription(
+ # [
+ # RegisterEventHandler(
+ # event_handler = OnProcessStart(
+ # target_action = navsat,
+ # on_start=[ekf_map,ekf_odom]
+ # )
+ # ),
+ # #ekf,
+ # navsat
+ #
+ # ]
+ [
+ navsat,
+ ekf_local,
+ ekf_global
+ ]
+ )