Feat/ekf

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
         ]
     )

urc_hw_description/urdf/walli_sensors.xacro

@@ -38,24 +38,24 @@
             <noise type="gaussian">
               <mean>0.0</mean>
               <stddev>2e-4</stddev>
-              <bias_mean>0.0000075</bias_mean>
-              <bias_stddev>0.0000008</bias_stddev>
+              <bias_mean>0.0</bias_mean>
+              <bias_stddev>0.0</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>
+              <bias_mean>0.0</bias_mean>
+              <bias_stddev>0.0</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>
+              <bias_mean>0.0</bias_mean>
+              <bias_stddev>0.0</bias_stddev>
             </noise>
           </z>
         </angular_velocity>
@@ -64,24 +64,24 @@
             <noise type="gaussian">
               <mean>0.0</mean>
               <stddev>1.7e-2</stddev>
-              <bias_mean>0.1</bias_mean>
-              <bias_stddev>0.001</bias_stddev>
+              <bias_mean>0.0</bias_mean>
+              <bias_stddev>0.0</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>
+              <bias_mean>0.0</bias_mean>
+              <bias_stddev>0.0</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>
+              <bias_mean>0.0</bias_mean>
+              <bias_stddev>0.0</bias_stddev>
             </noise>
           </z>
         </linear_acceleration>
@@ -116,7 +116,7 @@
           <namespace>/gps</namespace>
           <remapping>~/out:=data</remapping>
         </ros>
-        <frame_name>gps</frame_name>
+        <frame_name>map</frame_name>
       </plugin>
       <always_on>true</always_on>
       <updateRate>30</updateRate>
@@ -347,4 +347,4 @@
       <gaussian_noise>0.00</gaussian_noise>
     </plugin>
   </gazebo>
-</robot>
+</robot>