Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Completely rewritten spin, backup, and drive on heading tests to remove flakiness #4515

Merged
merged 9 commits into from
Jul 3, 2024
Prev Previous commit
Next Next commit
port drive on heading behavior to new gazebo
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
  • Loading branch information
stevedanomodolor committed Jun 24, 2024
commit 2861ce37c0f3128d0b9789510ab218d07fb76a94
2 changes: 1 addition & 1 deletion nav2_system_tests/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +124,7 @@ if(BUILD_TESTING)
# add_subdirectory(src/behaviors/spin)
# add_subdirectory(src/behaviors/wait)
# add_subdirectory(src/behaviors/backup)
# add_subdirectory(src/behaviors/drive_on_heading)
add_subdirectory(src/behaviors/drive_on_heading)
# add_subdirectory(src/behaviors/assisted_teleop)
# add_subdirectory(src/costmap_filters)
add_subdirectory(src/error_codes)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,9 +15,6 @@ ament_add_test(test_drive_on_heading_recovery
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
TIMEOUT 180
ENV
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
TEST_EXECUTABLE=$<TARGET_FILE:${test_drive_on_heading_behavior}>
TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
)
Original file line number Diff line number Diff line change
Expand Up @@ -14,13 +14,15 @@
# limitations under the License.

import os
from pathlib import Path
import sys

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch import LaunchService
from launch.actions import (
AppendEnvironmentVariable,
ExecuteProcess,
IncludeLaunchDescription,
SetEnvironmentVariable,
Expand All @@ -30,20 +32,29 @@
from launch_testing.legacy import LaunchTestService

from nav2_common.launch import RewrittenYaml
from nav2_simple_commander.utils import kill_os_processes


def generate_launch_description():
map_yaml_file = os.getenv('TEST_MAP')
world = os.getenv('TEST_WORLD')
sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
ros_gz_sim_dir = get_package_share_directory('ros_gz_sim')

world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro')

urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
with open(urdf, 'r') as infp:
robot_description = infp.read()

map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
bt_navigator_xml = os.path.join(
get_package_share_directory('nav2_bt_navigator'),
'behavior_trees',
os.getenv('BT_NAVIGATOR_XML'),
)

bringup_dir = get_package_share_directory('nav2_bringup')
params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml')
params_file = os.path.join(nav2_bringup_dir, 'params/nav2_params.yaml')

# Replace the `use_astar` setting on the params file
configured_params = RewrittenYaml(
Expand All @@ -54,34 +65,46 @@ def generate_launch_description():
[
SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
# Launch gazebo server for simulation
ExecuteProcess(
cmd=[
'gzserver',
'-s',
'libgazebo_ros_init.so',
'--minimal_comms',
world,
],
output='screen',
AppendEnvironmentVariable(
'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
),
# TODO(orduno) Launch the robot state publisher instead
# using a local copy of TB3 urdf file
Node(
package='tf2_ros',
executable='static_transform_publisher',
output='screen',
arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'],
AppendEnvironmentVariable(
'GZ_SIM_RESOURCE_PATH',
str(Path(os.path.join(sim_dir)).parent.resolve())
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py')
),
launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')
),
launch_arguments={
'use_sim_time': 'True',
'robot_sdf': robot_sdf,
'x_pose': '-2.0',
'y_pose': '-0.5',
'z_pose': '0.01',
'roll': '0.0',
'pitch': '0.0',
'yaw': '0.0',
}.items(),
),
Node(
package='tf2_ros',
executable='static_transform_publisher',
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'],
parameters=[
{'use_sim_time': True, 'robot_description': robot_description}
],
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(bringup_dir, 'launch', 'bringup_launch.py')
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')
),
launch_arguments={
'map': map_yaml_file,
Expand Down Expand Up @@ -111,7 +134,9 @@ def main(argv=sys.argv[1:]):
lts.add_test_action(ld, test1_action)
ls = LaunchService(argv=argv)
ls.include_launch_description(ld)
return lts.run(ls)
return_code = lts.run(ls)
kill_os_processes('gz sim')
return return_code


if __name__ == '__main__':
Expand Down
Loading