diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt
index 804e94f9d97d73b3cac05b20c7fa10cfea45fbc2..f69405437dfb52654158c82fa182dab14835bf5c 100644
--- a/nav2_system_tests/src/system/CMakeLists.txt
+++ b/nav2_system_tests/src/system/CMakeLists.txt
@@ -25,3 +25,17 @@ ament_add_test(test_bt_navigator_with_dijkstra
BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml
ASTAR=False
)
+
+ament_add_test(test_dynamic_obstacle
+ GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+ COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+ WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+ TIMEOUT 120
+ ENV
+ TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+ TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+ TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world
+ GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+ BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml
+ ASTAR=True
+)
diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo.world
index b47308dfad01ab2e455550712e12344dc0db39f0..461dd137daf7652c43dde78705bb4cb7ae6d78ab 100644
--- a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world
+++ b/nav2_system_tests/worlds/turtlebot3_ros2_demo.world
@@ -1,18 +1,4 @@
<?xml version="1.0"?>
-<!--
- Gazebo ROS turtlebot3 demo
-
- Run teleop node:
-
- ros2 run turtlebot3_teleop turtlebot3_teleop_key
-
- Try sending commands:
-
- ros2 topic pub /tb3/cmd_vel geometry_msgs/Twist '{linear: {x: 1.0}}' -1
-
- ros2 topic pub /tb3/cmd_vel geometry_msgs/Twist '{angular: {z: 0.1}}' -1
-
--->
<sdf version="1.6">
<world name="default">
diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world
new file mode 100644
index 0000000000000000000000000000000000000000..0ae12a416a3633c2e00da695217197dfea6c3f82
--- /dev/null
+++ b/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world
@@ -0,0 +1,523 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+ <world name="default">
+
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <scene>
+ <shadows>false</shadows>
+ </scene>
+
+ <gui fullscreen='0'>
+ <camera name='user_camera'>
+ <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
+ <view_controller>orbit</view_controller>
+ <projection_type>perspective</projection_type>
+ </camera>
+ </gui>
+
+ <physics type="ode">
+ <real_time_update_rate>1000.0</real_time_update_rate>
+ <max_step_size>0.001</max_step_size>
+ <real_time_factor>1</real_time_factor>
+ <ode>
+ <solver>
+ <type>quick</type>
+ <iters>150</iters>
+ <precon_iters>0</precon_iters>
+ <sor>1.400000</sor>
+ <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
+ </solver>
+ <constraints>
+ <cfm>0.00001</cfm>
+ <erp>0.2</erp>
+ <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
+ <contact_surface_layer>0.01000</contact_surface_layer>
+ </constraints>
+ </ode>
+ </physics>
+
+ <model name="turtlebot3_world">
+ <static>1</static>
+ <include>
+ <uri>model://turtlebot3_world</uri>
+ </include>
+ </model>
+
+ <model name="turtlebot3">
+ <pose>-2.0 -0.5 0.01 0.0 0.0 0.0</pose>
+
+ <link name="base_footprint"/>
+
+ <link name="base_link">
+
+ <inertial>
+ <pose>-0.032 0 0.070 0 0 0</pose>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0.000</ixy>
+ <ixz>0.000</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0.000</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ <mass>1.0</mass>
+ </inertial>
+
+ <collision name="base_collision">
+ <pose>-0.032 0 0.070 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.140 0.140 0.140</size>
+ </box>
+ </geometry>
+ </collision>
+
+ <visual name="base_visual">
+ <pose>-0.032 0 0 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <uri>model://turtlebot3_burger/meshes/burger_base.dae</uri>
+ <scale>0.001 0.001 0.001</scale>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <link name="imu_link">
+ <sensor name="tb3_imu" type="imu">
+ <always_on>true</always_on>
+ <update_rate>200</update_rate>
+ <imu>
+ <angular_velocity>
+ <x>
+ <noise type="gaussian">
+ <mean>0.0</mean>
+ <stddev>2e-4</stddev>
+ </noise>
+ </x>
+ <y>
+ <noise type="gaussian">
+ <mean>0.0</mean>
+ <stddev>2e-4</stddev>
+ </noise>
+ </y>
+ <z>
+ <noise type="gaussian">
+ <mean>0.0</mean>
+ <stddev>2e-4</stddev>
+ </noise>
+ </z>
+ </angular_velocity>
+ <linear_acceleration>
+ <x>
+ <noise type="gaussian">
+ <mean>0.0</mean>
+ <stddev>1.7e-2</stddev>
+ </noise>
+ </x>
+ <y>
+ <noise type="gaussian">
+ <mean>0.0</mean>
+ <stddev>1.7e-2</stddev>
+ </noise>
+ </y>
+ <z>
+ <noise type="gaussian">
+ <mean>0.0</mean>
+ <stddev>1.7e-2</stddev>
+ </noise>
+ </z>
+ </linear_acceleration>
+ </imu>
+ <plugin name="my_imu_plugin" filename="libgazebo_ros_imu_sensor.so">
+ <ros>
+ <!-- <namespace>/tb3</namespace> -->
+ <argument>--ros-args</argument>
+ <argument>-r</argument>
+ <argument>~/out:=imu</argument>
+ </ros>
+ </plugin>
+ </sensor>
+ </link>
+
+ <link name="base_scan">
+ <inertial>
+ <pose>-0.020 0 0.161 0 0 0</pose>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0.000</ixy>
+ <ixz>0.000</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0.000</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ <mass>0.125</mass>
+ </inertial>
+
+ <collision name="lidar_sensor_collision">
+ <pose>-0.020 0 0.161 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.0508</radius>
+ <length>0.055</length>
+ </cylinder>
+ </geometry>
+ </collision>
+
+ <visual name="lidar_sensor_visual">
+ <pose>-0.032 0 0.171 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <uri>model://turtlebot3_burger/meshes/lds.dae</uri>
+ <scale>0.001 0.001 0.001</scale>
+ </mesh>
+ </geometry>
+ </visual>
+
+ <sensor name="hls_lfcd_lds" type="ray">
+ <always_on>true</always_on>
+ <visualize>false</visualize>
+ <pose>-0.032 0 0.171 0 0 0</pose>
+ <update_rate>5</update_rate>
+ <ray>
+ <scan>
+ <horizontal>
+ <samples>360</samples>
+ <resolution>1.000000</resolution>
+ <min_angle>0.000000</min_angle>
+ <max_angle>6.280000</max_angle>
+ </horizontal>
+ </scan>
+ <range>
+ <min>0.120000</min>
+ <max>3.5</max>
+ <resolution>0.015000</resolution>
+ </range>
+ <noise>
+ <type>gaussian</type>
+ <mean>0.0</mean>
+ <stddev>0.01</stddev>
+ </noise>
+ </ray>
+ <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
+ <ros>
+ <!-- <namespace>/tb3</namespace> -->
+ <argument>--ros-args</argument>
+ <argument>-r </argument>
+ <argument>~/out:=scan</argument>
+ </ros>
+ <output_type>sensor_msgs/LaserScan</output_type>
+ </plugin>
+ </sensor>
+ </link>
+
+ <link name="wheel_left_link">
+
+ <inertial>
+ <pose>0 0.08 0.023 -1.57 0 0</pose>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0.000</ixy>
+ <ixz>0.000</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0.000</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ <mass>0.1</mass>
+ </inertial>
+
+ <collision name="wheel_left_collision">
+ <pose>0 0.08 0.023 -1.57 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.033</radius>
+ <length>0.018</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <!-- This friction pamareter don't contain reliable data!! -->
+ <friction>
+ <ode>
+ <mu>100000.0</mu>
+ <mu2>100000.0</mu2>
+ <fdir1>0 0 0</fdir1>
+ <slip1>0.0</slip1>
+ <slip2>0.0</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <soft_cfm>0</soft_cfm>
+ <soft_erp>0.2</soft_erp>
+ <kp>1e+5</kp>
+ <kd>1</kd>
+ <max_vel>0.01</max_vel>
+ <min_depth>0.001</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+
+ <visual name="wheel_left_visual">
+ <pose>0 0.08 0.023 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <uri>model://turtlebot3_burger/meshes/left_tire.dae</uri>
+ <scale>0.001 0.001 0.001</scale>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <link name="wheel_right_link">
+
+ <inertial>
+ <pose>0.0 -0.08 0.023 -1.57 0 0</pose>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0.000</ixy>
+ <ixz>0.000</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0.000</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ <mass>0.1</mass>
+ </inertial>
+
+ <collision name="wheel_right_collision">
+ <pose>0.0 -0.08 0.023 -1.57 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.033</radius>
+ <length>0.018</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <!-- This friction pamareter don't contain reliable data!! -->
+ <friction>
+ <ode>
+ <mu>100000.0</mu>
+ <mu2>100000.0</mu2>
+ <fdir1>0 0 0</fdir1>
+ <slip1>0.0</slip1>
+ <slip2>0.0</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <soft_cfm>0</soft_cfm>
+ <soft_erp>0.2</soft_erp>
+ <kp>1e+5</kp>
+ <kd>1</kd>
+ <max_vel>0.01</max_vel>
+ <min_depth>0.001</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+
+ <visual name="wheel_right_visual">
+ <pose>0.0 -0.08 0.023 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <uri>model://turtlebot3_burger/meshes/right_tire.dae</uri>
+ <scale>0.001 0.001 0.001</scale>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <link name='caster_back_link'>
+ <pose>-0.081 0 -0.004 0 0 0</pose>
+ <inertial>
+ <mass>0.001</mass>
+ <inertia>
+ <ixx>0.00001</ixx>
+ <ixy>0.000</ixy>
+ <ixz>0.000</ixz>
+ <iyy>0.00001</iyy>
+ <iyz>0.000</iyz>
+ <izz>0.00001</izz>
+ </inertia>
+ </inertial>
+ <collision name='collision'>
+ <geometry>
+ <sphere>
+ <radius>0.005000</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <contact>
+ <ode>
+ <soft_cfm>0</soft_cfm>
+ <soft_erp>0.2</soft_erp>
+ <kp>1e+5</kp>
+ <kd>1</kd>
+ <max_vel>0.01</max_vel>
+ <min_depth>0.001</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ </link>
+
+ <joint name="base_joint" type="fixed">
+ <parent>base_footprint</parent>
+ <child>base_link</child>
+ <pose>0.0 0.0 0.010 0 0 0</pose>
+ </joint>
+
+ <joint name="wheel_left_joint" type="revolute">
+ <parent>base_link</parent>
+ <child>wheel_left_link</child>
+ <pose>0.0 0.08 0.023 -1.57 0 0</pose>
+ <axis>
+ <xyz>0 0 1</xyz>
+ </axis>
+ </joint>
+
+ <joint name="wheel_right_joint" type="revolute">
+ <parent>base_link</parent>
+ <child>wheel_right_link</child>
+ <pose>0.0 -0.08 0.023 -1.57 0 0</pose>
+ <axis>
+ <xyz>0 0 1</xyz>
+ </axis>
+ </joint>
+
+ <joint name='caster_back_joint' type='ball'>
+ <parent>base_link</parent>
+ <child>caster_back_link</child>
+ </joint>
+
+ <joint name="imu_joint" type="fixed">
+ <parent>base_link</parent>
+ <child>imu_link</child>
+ <pose>-0.032 0 0.068 0 0 0</pose>
+ <axis>
+ <xyz>0 0 1</xyz>
+ </axis>
+ </joint>
+
+ <joint name="lidar_joint" type="fixed">
+ <parent>base_link</parent>
+ <child>base_scan</child>
+ <pose>-0.032 0 0.171 0 0 0</pose>
+ <axis>
+ <xyz>0 0 1</xyz>
+ </axis>
+ </joint>
+
+ <plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so">
+
+ <ros>
+ <!-- <namespace>/tb3</namespace> -->
+ </ros>
+
+ <!-- wheels -->
+ <left_joint>wheel_left_joint</left_joint>
+ <right_joint>wheel_right_joint</right_joint>
+
+ <!-- kinematics -->
+ <wheel_separation>0.160</wheel_separation>
+ <wheel_diameter>0.066</wheel_diameter>
+
+ <!-- limits -->
+ <max_wheel_torque>20</max_wheel_torque>
+ <max_wheel_acceleration>1.0</max_wheel_acceleration>
+
+ <!-- output -->
+ <publish_odom>true</publish_odom>
+ <publish_odom_tf>true</publish_odom_tf>
+ <publish_wheel_tf>false</publish_wheel_tf>
+
+ <odometry_frame>odom</odometry_frame>
+ <robot_base_frame>base_footprint</robot_base_frame>
+
+ </plugin>
+
+ <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
+ <ros>
+ <!-- <namespace>/tb3</namespace> -->
+ <argument>--ros-args</argument>
+ <argument>-r </argument>
+ <argument>~/out:=joint_states</argument>
+ </ros>
+ <update_rate>250</update_rate>
+ <joint_name>wheel_left_joint</joint_name>
+ <joint_name>wheel_right_joint</joint_name>
+ </plugin>
+
+ </model>
+
+ <model name='cardboard_box'>
+ <pose frame=''>-1.0 0.6 0.15 0 -0 0</pose>
+
+ <link name='link'>
+ <inertial>
+ <mass>20</mass>
+ <inertia>
+ <ixx>0.0416667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.0566667</iyy>
+ <iyz>0</iyz>
+ <izz>0.0683333</izz>
+ </inertia>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ </inertial>
+
+ <collision name='collision'>
+ <geometry>
+ <box>
+ <size>0.5 0.4 0.3</size>
+ </box>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>1</mu>
+ <mu2>1</mu2>
+ </ode>
+ <torsional>
+ <ode/>
+ </torsional>
+ </friction>
+ <contact>
+ <ode>
+ <kp>1e+07</kp>
+ <kd>1</kd>
+ <min_depth>0.001</min_depth>
+ <max_vel>0.1</max_vel>
+ </ode>
+ </contact>
+ <bounce/>
+ </surface>
+ <max_contacts>10</max_contacts>
+ </collision>
+
+ <visual name='visual'>
+ <pose frame=''>0 0 -0.15 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <uri>model://cardboard_box/meshes/cardboard_box.dae</uri>
+ <scale>1.25932 1.00745 0.755591</scale>
+ </mesh>
+ </geometry>
+ </visual>
+
+ <self_collide>0</self_collide>
+ <enable_wind>0</enable_wind>
+ <kinematic>0</kinematic>
+ </link>
+ </model>
+
+ </world>
+</sdf>
diff --git a/tools/run_test_suite.bash b/tools/run_test_suite.bash
index 3a156d840f7a8a403f845b965e4ad4cf87b96147..df3480e53799a02072bdf2ec383ff3f5ced156fe 100755
--- a/tools/run_test_suite.bash
+++ b/tools/run_test_suite.bash
@@ -24,3 +24,4 @@ $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_planner_cos
$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_planner_random$
$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator$
$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator_with_dijkstra$
+$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_dynamic_obstacle$