Updates README.

Signed-off-by: Franco Cipollone <[email protected]>

README.md

@@ -53,9 +53,9 @@ Once the package is built and sourced, you can start a simulation.
   ros2 launch andino_gz andino_gz.launch.py
   ```
 
-_Note: You can use `--world_name` flag to indicate other [world](andino_gz/worlds/) to use. (For example: `depot.sdf`(default), `empty.sdf`)_
+_Note: You can use `--world_name` flag to indicate other [world](andino_gz/worlds/) to use. (For example: `depot.sdf`(default), `office.sdf`, `empty.sdf`)_
 
-By default the ros bridge and rviz is initialized. In case you prefer to disable any of those you can do it via its flags:
+By default the ros bridge and rviz are initialized. In case you prefer to disable any of those you can do it via its flags:
 
   ```sh
   ros2 launch andino_gz andino_gz.launch.py ros_bridge:=false rviz:=false
@@ -84,6 +84,7 @@ Also, consider using looking at the translation entries under `andino_gz/config/
   ```
 
   _Note: You can add as many as you want_
+  _Note2: When running with Nav2(Using nav2:=True) start easy with 2 robots and see how it performs in your system before usingm more robots._
 
   <img src="./docs/media/andino_gz_multi_robot.png" width="800"/>
 
@@ -102,6 +103,36 @@ Also, consider using looking at the translation entries under `andino_gz/config/
   Note: A ROS Namespace is pushed for each robot so all the topics and nodes are called the same with a difference of a `<robot_name>` prefix.
   Note2: In order to RViz2 to work with the ROS namespace that is pushed, one has to make sure that no absolute topics are being used.
 
+### Navigation
+
+<img src="./docs/media/andino_nav2.png" width="800"/>
+
+1. Run simulation and enable Nav2
+
+    ```sh
+    ros2 launch andino_gz andino_gz.launch.py nav2:=True
+    ```
+
+    As before, you can launch as many robots as you want, for example launching two:
+
+    ```sh
+    ros2 launch andino_gz andino_gz.launch.py nav2:=True robots:="andino1={x: 0.0, y: 0.0, z: 0.1, yaw: 0.};andino2={x: 1.0, y: 0.0, z: 0.1, yaw: 0.};" 
+    ```
+
+    Once Gazebo window pops up, play the simulation using the gui.
+
+2. An RViz window will be spawned for each robot so it can be controlled independently. Use `2D Pose Estimate` to pass a hint to AMCL where is the initial point
+
+    Note you have to do it per robot, namely, per RViz window.
+
+
+    <img src="./docs/media/andino_nav2_pose_estimate.png" width="800"/>
+
+3. Send the desired goal using Nav2 Goal
+
+    <img src="./docs/media/andino_nav2_goal_pose.png" width="800"/>
+
+
 ### SLAM
 
 <img src="./docs/media/andino_slam.png" width="800"/>

andino_gz/launch/andino_gz.launch.py

@@ -19,8 +19,8 @@ def generate_launch_description():
     pkg_nav2_bringup = get_package_share_directory('nav2_bringup')
 
     ros_bridge_arg = DeclareLaunchArgument(
-        'ros_bridge', default_value='true', description='Run ROS bridge node.')
-    rviz_arg = DeclareLaunchArgument('rviz', default_value='true', description='Start RViz.')
+        'ros_bridge', default_value='True', description='Run ROS bridge node.')
+    rviz_arg = DeclareLaunchArgument('rviz', default_value='True', description='Start RViz.')
     world_name_arg = DeclareLaunchArgument(
         'world_name', default_value='depot.sdf', description='Name of the world to load. Match with map if using Nav2.')
     robots_arg = DeclareLaunchArgument(
@@ -31,7 +31,7 @@ def generate_launch_description():
         default_value='default.config',
         description='Name of the gui configuration file to load.')
     nav2_arg = DeclareLaunchArgument(
-        'nav2', default_value='True',
+        'nav2', default_value='False',
         description='Enable Nav2 Bringup.')
     map_name_arg = DeclareLaunchArgument(
       'map', default_value="depot", description='Name of the map to load. It should match the world_name.'
@@ -114,6 +114,7 @@ def generate_launch_description():
             launch_configurations={
                 'rviz': rviz,
                 'ros_bridge': ros_bridge,
+                'nav2': nav2_flag,
             },
             actions=[
                 LogInfo(msg="Group for robot: " + robot_name),
@@ -135,9 +136,9 @@ def generate_launch_description():
                         'use_sim_time': 'true',
                     }.items(),
                 ),
-                # RViz
+                # RViz with nav2
                 Node(
-                    condition=IfCondition(rviz),
+                    condition=IfCondition(PythonExpression([rviz, ' and ', LaunchConfiguration('nav2')])),
                     package='rviz2',
                     executable='rviz2',
                     arguments=['-d', os.path.join(pkg_andino_gz, 'rviz', 'andino_gz_nav2.rviz')],
@@ -147,6 +148,18 @@ def generate_launch_description():
                         ('/tf_static', 'tf_static'),
                     ],
                 ),
+                # RViz without nav2
+                Node(
+                    condition=IfCondition(PythonExpression([rviz, ' and not ', LaunchConfiguration('nav2')])),
+                    package='rviz2',
+                    executable='rviz2',
+                    arguments=['-d', os.path.join(pkg_andino_gz, 'rviz', 'andino_gz.rviz')],
+                    parameters=[{'use_sim_time': True}],
+                    remappings=[
+                        ('/tf', 'tf'),
+                        ('/tf_static', 'tf_static'),
+                    ],
+                ),
                 # Run ros_gz bridge
                 IncludeLaunchDescription(
                     PythonLaunchDescriptionSource(
@@ -182,8 +195,7 @@ def generate_launch_description():
                     'use_sim_time': 'True',
                     'params_file': LaunchConfiguration('params_file'),
                   }.items(),
-                  condition=IfCondition(more_than_one_robot),
-                  # condition=IfCondition(more_than_one_robot and LaunchConfiguration('nav2')),
+                  condition=IfCondition(PythonExpression([more_than_one_robot, ' and ', LaunchConfiguration('nav2')])),
               ),
               # Nav2 Bringup for single robot
               IncludeLaunchDescription(
@@ -196,11 +208,10 @@ def generate_launch_description():
                     'use_sim_time': 'True',
                     'params_file': LaunchConfiguration('params_file'),
                   }.items(),
-                  condition=IfCondition(one_robot),
-                  # condition=IfCondition(one_robot and LaunchConfiguration('nav2')),
+                  condition=IfCondition(PythonExpression([one_robot, ' and ', LaunchConfiguration('nav2')])),
               ),
-          ]
-        )
+            ]
+          )
         spawn_robots_group.append(robots_group)
         spawn_robots_group.append(nav_group)
 

andino_gz/package.xml

@@ -16,11 +16,12 @@
   <exec_depend>andino_slam</exec_depend>
 
   <exec_depend>ignition-gazebo6</exec_depend>
+  <exec_depend>nav2_bringup</exec_depend>
+  <exec_depend>nav2_common</exec_depend>
   <exec_depend>ros_gz_bridge</exec_depend>
   <exec_depend>ros_gz_sim</exec_depend>
 
   <exec_depend>ros2launch</exec_depend>
-  <exec_depend>nav2_common</exec_depend>
   <exec_depend>rviz2</exec_depend>
   <exec_depend>xacro</exec_depend>
 

andino_gz/rviz/andino_gz_nav2.rviz

@@ -586,7 +586,7 @@ Window Geometry:
     collapsed: false
   Navigation 2:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001d1000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fc00000212000001770000011200fffffffa000000010100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb00000018004e0061007600690067006100740069006f006e0020003201000000000000016a000000f700fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000000000010000017f0000034efc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0049006d006100670065010000003b0000009e0000002800fffffffb0000000a0056006900650077007301000000df000002aa000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003140000034e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001d1000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320100000212000001770000011200fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000000000010000017f0000034efc0200000005fb0000000a0049006d006100670065010000003b0000034e0000002800fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0049006d006100670065010000003b0000009e0000000000000000fb0000000a00560069006500770073000000003b0000034e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003140000034e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties: