Fix and cleanup e-puck example

webots_ros2/CHANGELOG.rst

@@ -13,6 +13,8 @@ Changelog for package webots_ros2
 * Changed undefined Lidar frequency to the default from the .proto file
 * Added Compass support in webots_ros2_driver
 * Added startup of the Turlebot navigation and mapping tools from the launch file.
+* Fixed the calibration of the e-puck.
+* Fixed and improved the navigation of the e-puck example.
 
 2023.0.2 (2023-02-07)
 ------------------

webots_ros2_epuck/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package webots_ros2_epuck
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2023.0.3 (2023-XX-XX)
+------------------
+* Refactored launch files.
+* Updated and fixed navigation for Humble compatibility.
+
 2023.0.2 (2023-02-07)
 ------------------
 * Updated supervisor launch.

webots_ros2_epuck/launch/rats_life_launch.py

@@ -23,14 +23,12 @@
 from launch import LaunchDescription
 from ament_index_python.packages import get_package_share_directory, get_packages_with_prefixes
 from launch_ros.actions import Node
-from launch.actions import ExecuteProcess
 
 
 def generate_launch_description():
     launch_description_nodes = []
     package_dir = get_package_share_directory('webots_ros2_epuck')
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
-    synchronization = LaunchConfiguration('synchronization', default=True)
     world = LaunchConfiguration('world', default='rats_life_benchmark.wbt')
 
     # Webots node
@@ -40,7 +38,6 @@ def generate_launch_description():
                 os.path.join(package_dir, 'launch', 'robot_launch.py')
             ),
             launch_arguments={
-                'synchronization': synchronization,
                 'use_sim_time': 'true',
                 'world': world
             }.items()
@@ -76,25 +73,6 @@ def generate_launch_description():
                 ],
             )
         )
-        # Set initial position of the robot within the provided map.
-        # The initial position can be also be set in RViz2 menu `2D Pose Estimate`.
-        launch_description_nodes.append(ExecuteProcess(
-            cmd=[
-                'ros2',
-                'topic',
-                'pub',
-                '--once',
-                '/initialpose',
-                'geometry_msgs/msg/PoseWithCovarianceStamped',
-                '{\
-                "header": { "frame_id": "map" },\
-                "pose": { "pose": {\
-                    "position": { "x": 0.005, "y": 0.0, "z": 0.0 },\
-                    "orientation": { "x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0 }}\
-                }\
-            }'
-            ]
-        ))
     else:
         launch_description_nodes.append(LogInfo(msg='Navigation2 is not installed, navigation functionality is disabled'))
 

webots_ros2_epuck/launch/rats_life_waypoints_launch.py

@@ -101,7 +101,6 @@ def get_waypoints():
 def generate_launch_description():
     package_dir = get_package_share_directory('webots_ros2_epuck')
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
-    synchronization = LaunchConfiguration('synchronization', default=True)
     world = LaunchConfiguration('world', default='rats_life_benchmark.wbt')
 
     # Webots
@@ -110,7 +109,6 @@ def generate_launch_description():
             os.path.join(package_dir, 'launch', 'robot_launch.py')
         ),
         launch_arguments={
-            'synchronization': synchronization,
             'use_sim_time': 'true',
             'world': world
         }.items()
@@ -153,7 +151,7 @@ def generate_launch_description():
         cmd=[
             'sleep 5;'
             'while [ -z '
-            f'`ros2 action send_goal /FollowWaypoints nav2_msgs/action/FollowWaypoints \'{get_waypoints()}\' | grep accepted`'
+            f'`ros2 action send_goal /follow_waypoints nav2_msgs/action/FollowWaypoints \'{get_waypoints()}\' | grep accepted`'
             ']; do'
             '  sleep 3;'
             'done'

webots_ros2_epuck/launch/robot_launch.py

@@ -20,17 +20,23 @@
 import pathlib
 import launch
 from launch.substitutions import LaunchConfiguration
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.substitutions.path_join_substitution import PathJoinSubstitution
 from launch_ros.actions import Node
 from launch import LaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from ament_index_python.packages import get_package_share_directory
 from webots_ros2_driver.webots_launcher import WebotsLauncher
 from webots_ros2_driver.utils import controller_url_prefix
 
 
 def get_ros2_nodes(*args):
     package_dir = get_package_share_directory('webots_ros2_epuck')
+    use_nav = LaunchConfiguration('nav', default=False)
+    use_rviz = LaunchConfiguration('rviz', default=False)
+    use_mapper = LaunchConfiguration('mapper', default=False)
+    fill_map = LaunchConfiguration('fill_map', default=True)
+    map_filename = LaunchConfiguration('map', default=os.path.join(package_dir, 'resource', 'epuck_world_map.yaml'))
     robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'epuck_webots.urdf')).read_text()
     ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml')
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
@@ -104,13 +110,53 @@ def get_ros2_nodes(*args):
         arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'],
     )
 
+    tool_nodes = []
+    # Navigation
+    nav_tools = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(package_dir, 'launch', 'robot_tools_launch.py')
+        ),
+        launch_arguments={
+            'nav': use_nav,
+            'rviz': use_rviz,
+            'use_sim_time': use_sim_time,
+            'map': map_filename,
+        }.items(),
+        condition=launch.conditions.IfCondition(use_nav)
+    )
+    tool_nodes.append(nav_tools)
+
+    # Mapping
+    mapper_tools = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(package_dir, 'launch', 'robot_tools_launch.py')
+        ),
+        launch_arguments={
+            'rviz': use_rviz,
+            'mapper': use_mapper,
+            'use_sim_time': use_sim_time,
+            'fill_map': fill_map,
+        }.items(),
+        condition=launch.conditions.IfCondition(use_mapper)
+    )
+    tool_nodes.append(mapper_tools)
+
+    # Wait for the simulation to be ready to start the tools
+    tools_handler = launch.actions.RegisterEventHandler(
+        event_handler=launch.event_handlers.OnProcessExit(
+            target_action=diffdrive_controller_spawner,
+            on_exit=tool_nodes
+        )
+    )
+
     return [
         joint_state_broadcaster_spawner,
         diffdrive_controller_spawner,
         robot_state_publisher,
         epuck_driver,
         footprint_publisher,
         epuck_process,
+        tools_handler,
     ]
 
 

webots_ros2_epuck/launch/robot_tools_launch.py

@@ -24,7 +24,6 @@
 from launch_ros.actions import Node
 from ament_index_python.packages import get_package_share_directory, get_packages_with_prefixes
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.actions import ExecuteProcess
 
 
 def generate_launch_description():
@@ -36,7 +35,7 @@ def generate_launch_description():
     use_rviz = LaunchConfiguration('rviz', default=True)
     use_mapper = LaunchConfiguration('mapper', default=False)
     fill_map = LaunchConfiguration('fill_map', default=True)
-    map_filename = LaunchConfiguration('fill_map', default=os.path.join(package_dir, 'resource', 'epuck_world_map.yaml'))
+    map_filename = LaunchConfiguration('map', default=os.path.join(package_dir, 'resource', 'epuck_world_map.yaml'))
 
     # Rviz node
     rviz_config = os.path.join(package_dir, 'resource', 'all.rviz')
@@ -66,23 +65,6 @@ def generate_launch_description():
                 condition=launch.conditions.IfCondition(use_nav)
             )
         )
-        launch_description_nodes.append(ExecuteProcess(
-            cmd=[
-                'ros2',
-                'topic',
-                'pub',
-                '--once',
-                '/initialpose',
-                'geometry_msgs/msg/PoseWithCovarianceStamped',
-                '{\
-                "header": { "frame_id": "map" },\
-                "pose": { "pose": {\
-                    "position": { "x": 0.005, "y": 0.0, "z": 0.0 },\
-                    "orientation": { "x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0 }}\
-                }\
-            }'
-            ]
-        ))
     else:
         launch_description_nodes.append(LogInfo(msg='Navigation2 is not installed, navigation functionality is disabled'))