Skip to content
Snippets Groups Projects
Unverified Commit caea1d2c authored by Carlos A. Orduno's avatar Carlos A. Orduno Committed by GitHub
Browse files

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.
parent f72007e2
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
nav2_system_tests/src/system/CMakeLists.txt
+ 14
− 0
View file @ caea1d2c
...@@ -25,3 +25,17 @@ ament_add_test(test_bt_navigator_with_dijkstra ...@@ -25,3 +25,17 @@ ament_add_test(test_bt_navigator_with_dijkstra
BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml
ASTAR=False 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
)
nav2_system_tests/worlds/turtlebot3_ros2_demo.world
+ 0
− 14
View file @ caea1d2c
<?xml version="1.0"?> <?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"> <sdf version="1.6">
<world name="default"> <world name="default">
......
nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world 0 → 100644
+ 523
− 0
View file @ caea1d2c
<?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>
tools/run_test_suite.bash
+ 1
− 0
View file @ caea1d2c
...@@ -24,3 +24,4 @@ $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_planner_cos ...@@ -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_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$
$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_bt_navigator_with_dijkstra$
$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_dynamic_obstacle$
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment