Nav2 now goes correctly to goal (at least in simulation)

src/ezbot_robot/CMakeLists.txt

@@ -33,20 +33,22 @@ if(BUILD_TESTING)
 endif()
 
 install(
-  DIRECTORY config description launch worlds meshes media
+  DIRECTORY config description launch worlds meshes media maps
   DESTINATION share/${PROJECT_NAME}
 )
 
 install(
-  DIRECTORY media/
-  DESTINATION $ENV{HOME}/.gazebo/media/
+  DIRECTORY maps/
+  DESTINATION $ENV{HOME}/ros2maps/
 )
 
 install(
   DIRECTORY media/
   DESTINATION $ENV{HOME}/.gazebo/media/
 )
 
+
+
 install(PROGRAMS
   launch/camerapubsub.py
   DESTINATION lib/${PROJECT_NAME})

src/ezbot_robot/config/nav2_params.yaml

@@ -69,11 +69,11 @@ controller_server:
   ros__parameters:
     controller_frequency: 20.0
     min_x_velocity_threshold: 0.001
-    min_y_velocity_threshold: 0.5
+    min_y_velocity_threshold: 0.001
     min_theta_velocity_threshold: 0.001
     failure_tolerance: 0.3
     progress_checker_plugins: ["progress_checker"]
-    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    goal_checker_plugins: ["precise_goal_checker"] # "precise_goal_checker"
     controller_plugins: ["FollowPath"]
     use_realtime_priority: false
 
@@ -83,11 +83,11 @@ controller_server:
       required_movement_radius: 0.5
       movement_time_allowance: 10.0
     # Goal checker parameters
-    #precise_goal_checker:
-    #  plugin: "nav2_controller::SimpleGoalChecker"
-    #  xy_goal_tolerance: 0.25
-    #  yaw_goal_tolerance: 0.25
-    #  stateful: True
+    precise_goal_checker:
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.02
+      yaw_goal_tolerance: 0.02
+      stateful: True
     general_goal_checker:
       stateful: True
       plugin: "nav2_controller::SimpleGoalChecker"

src/ezbot_robot/description/gazebo_control.xacro

@@ -7,6 +7,7 @@
             <West>roue_gauche_joint</West>
             <South>roue_arriere_joint</South>
             <East>roue_droite_joint</East>
+            <commandTopic>/omnidirectional_controller/cmd_vel_unstamped</commandTopic>
 
             <wheel_separation>0.25</wheel_separation>
             <wheel_diameter>0.06</wheel_diameter>

src/ezbot_robot/description/lidar.xacro

@@ -57,7 +57,7 @@
 
         <sensor name="laser" type="ray">
             <pose> 0.055 0 0.03 0 0 0 </pose>
-            <visualize>false</visualize>
+            <visualize>true</visualize>
             <update_rate>10</update_rate>
             <ray>
                 <scan>
@@ -74,7 +74,7 @@
             </ray>
             <plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
                 <ros>
-                    <argument>~/out:=scan</argument>
+                    <argument>~/out:=lidar/ldlidar_node/scan</argument>
                 </ros>
                 <output_type>sensor_msgs/LaserScan</output_type>
                 <frame_name>base_laser</frame_name>

src/ezbot_robot/description/robot.urdf.xacro

@@ -4,9 +4,7 @@
     <xacro:include filename="robot_core.xacro" />
     <xacro:include filename="lidar.xacro"/>
     <xacro:include filename="ros2_control.xacro" />
-    <!--
-        <xacro:include filename="gazebo_control.xacro" />
-    -->
+    <xacro:include filename="gazebo_control.xacro" />
     <xacro:include filename="camera.xacro" />
     <xacro:include filename="imu.xacro" />
 </robot>

src/ezbot_robot/launch/map_server.launch.py

@@ -0,0 +1,51 @@
+import os
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+from launch.substitutions import LaunchConfiguration
+import launch_ros.actions
+
+
+def generate_launch_description():
+
+    ld = LaunchDescription()
+
+    # map file
+    map_file_path = os.path.join(
+        get_package_share_directory('ezbot_robot'),
+        'maps',
+        'testMap',
+        'testMap.yaml'
+    )
+
+    map_server_cmd = Node(
+        package='nav2_map_server',
+        executable='map_server',
+        output='screen',
+        namespace='map',
+        parameters=[{'yaml_filename': map_file_path},
+                    {'frame_id': 'map'}],
+        remappings=[('/map/map', '/map')]
+    )
+
+
+    lifecycle_nodes = ['map_server']
+    autostart = True
+
+    start_lifecycle_manager_cmd = launch_ros.actions.Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            name='lifecycle_manager',
+            namespace='map',
+            output='screen',
+            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+            parameters=[{'autostart': autostart},
+                        {'node_names': lifecycle_nodes}])
+
+
+    ld.add_action(map_server_cmd)
+    ld.add_action(start_lifecycle_manager_cmd)
+
+    return ld

src/ezbot_robot/maps/testMap/testMap.yaml

@@ -0,0 +1,7 @@
+yaml_filename: "yaml_filename"
+image: blackandwhite.jpg
+resolution: 0.0100
+origin: [1.0, 2.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196