Armant/fix local ukf (#100)

* Change to work with imu_filter_madgwick instead of  mahony

* Put all launch stuff in one file

* fix urdf temporarily

Author: wflyerthariani

src/localization_dev/config/ukf_local.yaml

@@ -1,10 +1,12 @@
 ukf_local:
   ros__parameters:
     frequency: 50.0
-    sensor_timeout: 0.1
-    two_d_mode: true
+    two_d_mode: false
+
+    print_diagnostics: true
+    debug: true
+    debug_out_file: /home/arman/debug/debug_out_file.txt
 
-    map_frame: map
     odom_frame: odom
     base_link_frame: base_link
     world_frame: odom
@@ -15,10 +17,9 @@ ukf_local:
     imu0: /imu/data
     imu0_config: [
       false, false, false,   # position
-      true,  true,  true,    # orientation (Mahony)
+      true,  true,  false,    # orientation (Mahony)
       false, false, false,   # velocity
       true,  true,  true,    # angular velocity
-      true,  true,  true     # linear acceleration
+      false,  false,  false     # linear acceleration
     ]
-
     imu0_remove_gravitational_acceleration: false

src/localization_dev/launch/localization.launch.py

@@ -3,22 +3,17 @@
 from launch_ros.substitutions import FindPackageShare
 from launch.substitutions import PathJoinSubstitution
 
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
 def generate_launch_description():
     ukf_local_yaml = PathJoinSubstitution([
         FindPackageShare('localization_dev'),
         'config',
         'ukf_local.yaml'
     ])
-    ukf_global_yaml = PathJoinSubstitution([
-        FindPackageShare('localization_dev'),
-        'config',
-        'ukf_global.yaml'
-    ])
-    navsat_yaml = PathJoinSubstitution([
-        FindPackageShare('localization_dev'),
-        'config',
-        'navsat.yaml'
-    ])
     return LaunchDescription([
 
         Node(
@@ -29,20 +24,22 @@ def generate_launch_description():
             parameters=[ukf_local_yaml],
             remappings=[('odometry/filtered', 'odometry/filtered')]
         ),
-
         Node(
-            package='robot_localization',
-            executable='navsat_transform_node',
-            name='navsat_transform',
-            output='screen',
-            parameters=[navsat_yaml]
-        ),
-
-        Node(
-            package='robot_localization',
-            executable='ukf_node',
-            name='ukf_global',
+            package='imu_filter_madgwick',
+            executable='imu_filter_madgwick_node',
+            name='imu_filter',
             output='screen',
-            parameters=[ukf_global_yaml]
+            parameters=[{
+                'publish_tf': False
+            }]
         ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(
+                    get_package_share_directory('phidgets_spatial'),
+                    'launch',
+                    'spatial-launch.py'
+                )
+            )
+        )
     ])

src/rover_description/launch/display_chassis.launch.py

@@ -56,7 +56,7 @@ def generate_launch_description():
     )
 
     urdf_file_path = (
-        get_package_share_directory("rover_description") + "/urdf/chassis_urdf_24_rviz.urdf"
+        get_package_share_directory("rover_description") + "/urdf/Chassis_jan25.SLDASM.urdf"
     )
 
     rviz_node = Node(

src/rover_description/launch/slam_launch.py

@@ -15,11 +15,6 @@ def generate_launch_description():
                 "base_frame": "base_link",
                 "scan_topic": "/scan",
                 "mode": "mapping",
-                "transform_tolerance": 1,
-                "transform_cache_size": 10000,
-                "transform_publish_period": 0.05,
-                "min_laser_range": 0.0,
-                "max_laser_range": 25.0
             }]
         )
     ])

Chassis_jan25.SLDASM.urdf …scription/urdf/Chassis_jan25.SLDASM.urdfChassis_jan25.SLDASM.urdf renamed to src/rover_description/urdf/Chassis_jan25.SLDASM.urdf Chassis_jan25.SLDASM.urdf renamed to src/rover_description/urdf/Chassis_jan25.SLDASM.urdf

@@ -542,7 +542,7 @@
       xyz="0 0 0" />
   </joint>
   <link
-    name="IMU">
+    name="imu_link">
     <inertial>
       <origin
         xyz="0.00950000000000004 0.0125 0.00950000000000004"
@@ -590,8 +590,16 @@
     <parent
       link="base_link" />
     <child
-      link="IMU" />
+      link="imu_link" />
     <axis
       xyz="0 0 0" />
   </joint>
+  <link name="lidar">
+  </link>
+
+  <joint name="lidar_joint" type="fixed">
+    <parent link="base_link" />
+    <child link="lidar" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
 </robot>