From 5af65f1a5fd4d2627db72991266e14c0e85fbda2 Mon Sep 17 00:00:00 2001
From: TheNoobInventor <eduobijoro@mailfence.com>
Date: Fri, 13 Oct 2023 14:42:57 +0000
Subject: [PATCH] Update gazebo and lidarbot bringup launch files

---
 README.md                                     |  2 ++
 .../launch/lidarbot_bringup_launch.py         |  4 ++--
 lidarbot_gazebo/launch/gazebo_launch.py       | 23 ++++++++++++++++---
 lidarbot_navigation/config/nav2_params.yaml   |  2 +-
 4 files changed, 25 insertions(+), 6 deletions(-)

diff --git a/README.md b/README.md
index 424fb82..b1137d4 100644
--- a/README.md
+++ b/README.md
@@ -691,6 +691,8 @@ TODO:
     map_start_at_dock: true 
 ```
 
+TODO: Note about the option of using AMCL for localization using nav2's localization launch file. Currently using slam_toolbox for localization
+
 ### Lidarbot
 
 Change `map_file_name` key to use real map
diff --git a/lidarbot_bringup/launch/lidarbot_bringup_launch.py b/lidarbot_bringup/launch/lidarbot_bringup_launch.py
index 7f8e605..a883cb3 100644
--- a/lidarbot_bringup/launch/lidarbot_bringup_launch.py
+++ b/lidarbot_bringup/launch/lidarbot_bringup_launch.py
@@ -75,7 +75,7 @@ def generate_launch_description():
 
     # Spawn joint_state_broadcaser
     start_joint_broadcaster_cmd = Node(
-        condition=IfCondition(use_ros2_control),
+        # condition=IfCondition(use_ros2_control),
         package='controller_manager',
         executable='spawner',
         arguments=['joint_broadcaster'])
@@ -88,7 +88,7 @@ def generate_launch_description():
 
     # Spawn imu_sensor_broadcaser
     start_imu_broadcaster_cmd = Node(
-        condition=IfCondition(use_ros2_control),
+        # condition=IfCondition(use_ros2_control),
         package='controller_manager',
         executable='spawner',
         arguments=['imu_broadcaster'])
diff --git a/lidarbot_gazebo/launch/gazebo_launch.py b/lidarbot_gazebo/launch/gazebo_launch.py
index b4656e5..d9b0255 100644
--- a/lidarbot_gazebo/launch/gazebo_launch.py
+++ b/lidarbot_gazebo/launch/gazebo_launch.py
@@ -33,7 +33,9 @@ def generate_launch_description():
     use_sim_time = LaunchConfiguration('use_sim_time')
     use_ros2_control = LaunchConfiguration('use_ros2_control')
     world = LaunchConfiguration('world')
-    
+    use_robot_localization = LaunchConfiguration('use_robot_localization')
+
+    # Declare the launch arguments
     declare_use_sim_time_cmd = DeclareLaunchArgument(
         name='use_sim_time',
         default_value='True',
@@ -48,7 +50,12 @@ def generate_launch_description():
         name='world',
         default_value=world_path,
         description='Full path to the world model to load')
-    
+
+    declare_use_robot_localization_cmd = DeclareLaunchArgument(
+        name='use_robot_localization',
+        default_value='True',
+        description='Use robot_localization package if true')
+
     # Start robot state publisher
     start_robot_state_publisher_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([os.path.join(pkg_description, 'launch', 'robot_state_publisher_launch.py')]), 
@@ -74,6 +81,13 @@ def generate_launch_description():
         package='controller_manager',
         executable='spawner',
         arguments=['diff_controller'])
+   
+   # Spawn imu_sensor_broadcaser
+    start_imu_broadcaster_cmd = Node(
+        condition=IfCondition(use_ros2_control),
+        package='controller_manager',
+        executable='spawner',
+        arguments=['imu_broadcaster'])
 
     # Spawn joint_state_broadcaser
     start_joint_broadcaster_cmd = Node(
@@ -84,6 +98,7 @@ def generate_launch_description():
     
     # Start robot localization using an Extended Kalman Filter
     start_robot_localization_cmd = Node(
+        condition=IfCondition(use_robot_localization),
         package='robot_localization',
         executable='ekf_node',
         parameters=[ekf_params_file])
@@ -106,13 +121,15 @@ def generate_launch_description():
     ld.add_action(declare_use_sim_time_cmd)
     ld.add_action(declare_use_ros2_control_cmd)
     ld.add_action(declare_world_cmd)
-    
+    ld.add_action(declare_use_robot_localization_cmd)
+
     # Add any actions
     ld.add_action(start_robot_state_publisher_cmd)
     ld.add_action(start_gazebo_cmd)
     ld.add_action(start_spawner_cmd)
     ld.add_action(start_diff_controller_cmd)
     ld.add_action(start_joint_broadcaster_cmd)
+    ld.add_action(start_imu_broadcaster_cmd)
     ld.add_action(start_robot_localization_cmd)
     ld.add_action(start_joystick_cmd)
     ld.add_action(start_twist_mux_cmd)
diff --git a/lidarbot_navigation/config/nav2_params.yaml b/lidarbot_navigation/config/nav2_params.yaml
index 1ba776b..35d54e8 100644
--- a/lidarbot_navigation/config/nav2_params.yaml
+++ b/lidarbot_navigation/config/nav2_params.yaml
@@ -191,7 +191,7 @@ local_costmap:
       width: 3
       height: 3
       resolution: 0.05
-      robot_radius: 0.22
+      robot_radius: 0.14 #0.22
       plugins: ["voxel_layer", "inflation_layer"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"