From caea1d2cb748cebaf2b3444675226e8db14419c8 Mon Sep 17 00:00:00 2001
From: "Carlos A. Orduno" <39749557+orduno@users.noreply.github.com>
Date: Mon, 21 Oct 2019 16:04:00 -0700
Subject: [PATCH] Adding CI Test with obstacle not considered on the map
(#1259)
Adding a test that uses a world with a stationary obstacle that is not present on the map and that is blocking the preferred path. This is intended to test the obstacle layer. The robot should find an alternate path.
---
nav2_system_tests/src/system/CMakeLists.txt | 14 +
.../worlds/turtlebot3_ros2_demo.world | 14 -
.../turtlebot3_ros2_demo_obstacle.world | 523 ++++++++++++++++++
tools/run_test_suite.bash | 1 +
4 files changed, 538 insertions(+), 14 deletions(-)
create mode 100644 nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world
diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt
index 804e94f9..f6940543 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 b47308df..461dd137 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 00000000..0ae12a41
--- /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 3a156d84..df3480e5 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$
--
GitLab