Add use_localization and debug launch configurations. Fix Viewpoint.

mep3_bringup/launch/simulation_launch.py

@@ -7,36 +7,35 @@
 import launch
 from launch.actions import IncludeLaunchDescription
 from launch.actions.set_environment_variable import SetEnvironmentVariable
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 
 
 def generate_launch_description():
     use_behavior_tree = LaunchConfiguration('bt', default=True)
-    big_strategy = LaunchConfiguration('big_strategy', default='purple_strategy')
-    small_strategy = LaunchConfiguration('small_strategy', default='purple_strategy')
+    big_strategy = LaunchConfiguration('big_strategy',
+                                       default='purple_strategy')
+    small_strategy = LaunchConfiguration('small_strategy',
+                                         default='purple_strategy')
     color = LaunchConfiguration('color', default='purple')
     use_opponents = LaunchConfiguration('opponents', default=False)
     namespace = LaunchConfiguration('namespace', default='big')
+    debug = LaunchConfiguration('debug', default=False)
     use_localization = LaunchConfiguration('localization', default=False)
 
     set_color_action = SetEnvironmentVariable('MEP3_COLOR', color)
     set_use_opponents = SetEnvironmentVariable('MEP3_OPPONENTS', use_opponents)
 
     simulation = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(os.path.join(
-            get_package_share_directory('mep3_simulation'),
-            'launch',
-            'simulation_launch.py'
-        )),
-    )
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('mep3_simulation'),
+                         'launch', 'simulation_launch.py')),)
 
     big_robot = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(os.path.join(
-            get_package_share_directory('mep3_bringup'),
-            'launch',
-            'robot_launch.py'
-        )),
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('mep3_bringup'), 'launch',
+                         'robot_launch.py')),
         launch_arguments=[
             ('sim', 'true'),
             ('namespace', namespace),
@@ -47,11 +46,9 @@ def generate_launch_description():
     )
 
     small_robot = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(os.path.join(
-            get_package_share_directory('mep3_bringup'),
-            'launch',
-            'robot_launch.py'
-        )),
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('mep3_bringup'), 'launch',
+                         'robot_launch.py')),
         launch_arguments=[
             ('sim', 'true'),
             ('namespace', 'small'),
@@ -61,12 +58,23 @@ def generate_launch_description():
         ],
     )
 
+    localization = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('mep3_localization'),
+                         'launch', 'localization_launch.py')),
+        launch_arguments=[
+            ('debug', debug),
+        ],
+        condition=IfCondition(use_localization),
+    )
+
     return launch.LaunchDescription([
         big_robot,
         small_robot,
 
         # The easiest way to get pass variables to Webots controllers
         # is to use environment variables.
+        localization,
         set_color_action,
         set_use_opponents,
         simulation,

mep3_localization/launch/localization_launch.py

@@ -28,36 +28,36 @@ def generate_launch_description():
     TODO: add debug option for static marker positions
     """
     debug = LaunchConfiguration('debug', default=False)
+
     return LaunchDescription([
         Node(package='tf2_ros',
              executable='static_transform_publisher',
              arguments=[
-                 '-0.1', '1.50976', '1.05','-4.85921e-06', '-0.965927', '0.258816',
-                 '1.32679e-06', 'map', 'camera_prediction'
-             ])
+                 '-0.1', '1.50976', '1.05','-4.85921e-06', '-0.965927', '0.258816', '1.32679e-06', 'map', 'camera_prediction'
+             ]),
         Node(package='tf2_ros',
              executable='static_transform_publisher',
              arguments=[
                  '-0.430', '0.925', '0', '0', '0', '0', '1', 'map', 'marker_[20]_static'
              ],
-             condition = launch.contitions.IfCondition(debug)),
+             condition = IfCondition(debug)),
         Node(package='tf2_ros',
              executable='static_transform_publisher',
              arguments=[
                  '0.430', '0.925', '0', '0', '0', '0', '1', 'map', 'marker_[21]_static',
              ],
-             condition = launch.contitions.IfCondition(debug)),
+             condition = IfCondition(debug)),
         Node(package='tf2_ros',
              executable='static_transform_publisher',
              arguments=[
                  '-0.430', '-0.925', '0', '0', '0', '0', '1', 'map', 'marker_[22]_static',
              ],
-             condition = launch.contitions.IfCondition(debug)),
+             condition = IfCondition(debug)),
         Node(package='tf2_ros',
              executable='static_transform_publisher',
              arguments=[
                  '0.430', '-0.925', '0', '0', '0', '0', '1', 'map', 'marker_[23]_static',
              ],
-             condition = launch.contitions.IfCondition(debug)),
+             condition = IfCondition(debug)),
         Node(package='mep3_localization', executable='aruco_detector'),
     ])

mep3_simulation/launch/simulation_launch.py

@@ -4,6 +4,7 @@
 from ament_index_python.packages import get_package_share_directory
 import launch
 from launch.actions import IncludeLaunchDescription
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
@@ -21,8 +22,6 @@ def generate_launch_description():
         'controller_params_small',
         default=os.path.join(get_package_share_directory('mep3_bringup'),
                              'resource', 'ros2_control_small.yaml'))
-    debug = LaunchConfiguration('debug', default=False)
-    use_localization = LaunchConfiguration('localization', default=False)
 
     robot_description_big = pathlib.Path(
         os.path.join(package_dir, 'resource', 'config_big.urdf')).read_text()

mep3_simulation/webots_data/worlds/eurobot_2023.wbt

@@ -27,8 +27,8 @@ WorldInfo {
   ]
 }
 Viewpoint {
-  orientation 0.1784876953961999 0.07774536495607919 -0.9808658424167902 2.3340532829260487
-  position 1.6611698020954249 1.468483579255041 0.8481203223258266
+  orientation 0 1 0 1.57
+  position 0 0 5
 }
 TexturedBackground {
 }