From d50a0663022b7d2cadcce5e01cf1d8fb9ea06154 Mon Sep 17 00:00:00 2001 From: VladimirVincan Date: Tue, 22 Nov 2022 01:47:17 +0100 Subject: [PATCH] Add use_localization and debug launch configurations. Fix Viewpoint. --- mep3_bringup/launch/simulation_launch.py | 44 +++++++++++-------- .../launch/localization_launch.py | 16 +++---- mep3_simulation/launch/simulation_launch.py | 3 +- .../webots_data/worlds/eurobot_2023.wbt | 4 +- 4 files changed, 37 insertions(+), 30 deletions(-) diff --git a/mep3_bringup/launch/simulation_launch.py b/mep3_bringup/launch/simulation_launch.py index dd73fdcab..24af473ef 100644 --- a/mep3_bringup/launch/simulation_launch.py +++ b/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, diff --git a/mep3_localization/launch/localization_launch.py b/mep3_localization/launch/localization_launch.py index dfb127411..e2849e52c 100644 --- a/mep3_localization/launch/localization_launch.py +++ b/mep3_localization/launch/localization_launch.py @@ -25,39 +25,39 @@ def generate_launch_description(): It is necessary to have a prediction of the camera position in order to detect incorrect orientations of ArUco markers. - TODO: add debug option for static marker positions + Debug launch parameter is used to publish 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'), ]) diff --git a/mep3_simulation/launch/simulation_launch.py b/mep3_simulation/launch/simulation_launch.py index af47ce649..2f31070a3 100644 --- a/mep3_simulation/launch/simulation_launch.py +++ b/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() diff --git a/mep3_simulation/webots_data/worlds/eurobot_2023.wbt b/mep3_simulation/webots_data/worlds/eurobot_2023.wbt index b73bc0dd3..858ac1238 100644 --- a/mep3_simulation/webots_data/worlds/eurobot_2023.wbt +++ b/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 { }