Integrate Navigation2 package (#27)

* Added Navigation2

.gitattributes

@@ -4,5 +4,5 @@
 *.bmp filter=lfs diff=lfs merge=lfs -text
 *.svg filter=lfs diff=lfs merge=lfs -text
 *.pdf filter=lfs diff=lfs merge=lfs -text
-
+*.pgm filter=lfs diff=lfs merge=lfs -text
 . !text !filter !merge !diff

.gitignore

@@ -0,0 +1,2 @@
+.vscode/
+.idea/

README.md

@@ -17,10 +17,10 @@ git clone https://github.com/memristor/mep3.git ./foxy_ws/src/mep3
 ## Editing Webots world
 
 - Install [Webots R2021b](https://github.com/cyberbotics/webots/releases/download/R2021b/webots_2021b_amd64.deb)
-- Open [`webots_data/worlds/eurobot_2022.wbt`](./webots_data/worlds/eurobot_2022.wbt) in Webots
+- Open [`mep3_simulation/webots_data/worlds/eurobot_2022.wbt`](./mep3_simulation/webots_data/worlds/eurobot_2022.wbt) in Webots
 - Stop simulation and set time to `00:00:00`
 - Save changes
-- Commit all changes except for [`Viewpoint`](./webots_data/worlds/eurobot_2022.wbt#L5-L7)
+- Commit all changes except for [`Viewpoint`](./mep3_simulation/webots_data/worlds/eurobot_2022.wbt#L5-L7)
 
 ## ROS 2 setup
 
@@ -49,4 +49,11 @@ ros2 launch mep3_simulation robot_launch.py
 ```sh
 source /opt/ros/foxy/local_setup.bash
 ros2 run teleop_twist_keyboard teleop_twist_keyboard
+```
+
+### Navigation
+
+To launch simulation with `rviz` and `nav2` run
+```sh
+ros2 launch mep3_simulation robot_launch.py rviz:=true nav:=true
 ```

mep3_simulation/launch/robot_launch.py

@@ -2,6 +2,9 @@
 import pathlib
 import launch
 from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from ament_index_python.packages import get_package_share_directory
 from webots_ros2_core.webots_launcher import WebotsLauncher
 
@@ -38,9 +41,11 @@ def generate_launch_description():
         arguments=['joint_state_broadcaster'] + controller_manager_timeout,
     )
 
-    # The node which interacts with a robot in the Webots simulation is located in the `webots_ros2_driver` package under name `driver`.
+    # The node which interacts with a robot in the Webots simulation is located in the
+    # `webots_ros2_driver` package under name `driver`.
     # It is necessary to run such a node for each robot in the simulation.
-    # Typically, we provide it the `robot_description` parameters from a URDF file and `ros2_control_params` from the `ros2_control` configuration file.
+    # Typically, we provide it the `robot_description` parameters from a URDF file and
+    # `ros2_control_params` from the `ros2_control` configuration file.
     webots_robot_driver = Node(
         package='webots_ros2_driver',
         executable='driver',
@@ -53,9 +58,13 @@ def generate_launch_description():
         ]
     )
 
-    # Often we want to publish robot transforms, so we use the `robot_state_publisher` node for that.
-    # If robot model is not specified in the URDF file then Webots can help us with the URDF exportation feature.
-    # Since the exportation feature is available only once the simulation has started and the `robot_state_publisher` node requires a `robot_description` parameter before we have to specify a dummy robot.
+    # Often we want to publish robot transforms, so we use the `robot_state_publisher`
+    # node for that.
+    # If robot model is not specified in the URDF file then Webots can help us with the
+    # URDF exportation feature.
+    # Since the exportation feature is available only once the simulation has started and
+    # the `robot_state_publisher` node requires a `robot_description` parameter before we
+    # have to specify a dummy robot.
     robot_state_publisher = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
@@ -65,21 +74,69 @@ def generate_launch_description():
         }],
     )
 
+    use_rviz = LaunchConfiguration('rviz', default=False)
+    use_nav = LaunchConfiguration('nav', default=False)
+    use_sim_time = LaunchConfiguration('use_sim_time', default=True)
+    nav2_map = os.path.join(package_dir, 'resource', 'map.yml')
+
+    rviz_config = os.path.join(
+        get_package_share_directory('mep3_simulation'),
+        'resource',
+        'default.rviz'
+    )
+
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        output='screen',
+        arguments=['--display-config=' + rviz_config],
+        parameters=[{'use_sim_time': use_sim_time}],
+        condition=launch.conditions.IfCondition(use_rviz)
+    )
+
+    nav2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(
+            get_package_share_directory('nav2_bringup'),
+            'launch',
+            'bringup_launch.py'
+        )),
+        launch_arguments=[
+            ('map', nav2_map),
+            ('use_sim_time', use_sim_time),
+        ],
+        condition=launch.conditions.IfCondition(use_nav)
+    )
+
+    map_odom_publisher = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        output='screen',
+        arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']
+    )
+
     # Standard ROS 2 launch description
     return launch.LaunchDescription([
 
+        # Wheel controller
         joint_state_broadcaster_spawner,
         diffdrive_controller_spawner,
 
         # Start the Webots node
         webots,
 
+        # 3D Visualization
+        rviz,
+
         # Start the Webots robot driver
         webots_robot_driver,
 
         # Start the robot_state_publisher
         robot_state_publisher,
 
+        # Navigation 2
+        nav2,
+        map_odom_publisher,
+
         # This action will kill all nodes once the Webots simulation has exited
         launch.actions.RegisterEventHandler(
             event_handler=launch.event_handlers.OnProcessExit(

mep3_simulation/package.xml

@@ -13,6 +13,10 @@
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>
   <test_depend>python3-pytest</test_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>navigation2</exec_depend>
+  <exec_depend>nav2_bringup</exec_depend>
   <export>
     <build_type>ament_python</build_type>
   </export>