add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,74 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(nav2_system_tests)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(nav2_common REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
find_package(nav2_map_server REQUIRED)
|
||||
find_package(nav2_behavior_tree REQUIRED)
|
||||
find_package(nav2_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(gazebo_ros_pkgs REQUIRED)
|
||||
find_package(nav2_amcl REQUIRED)
|
||||
find_package(nav2_lifecycle_manager REQUIRED)
|
||||
find_package(rclpy REQUIRED)
|
||||
find_package(nav2_navfn_planner REQUIRED)
|
||||
find_package(nav2_planner REQUIRED)
|
||||
find_package(navigation2)
|
||||
find_package(angles REQUIRED)
|
||||
find_package(behaviortree_cpp_v3 REQUIRED)
|
||||
|
||||
nav2_package()
|
||||
|
||||
set(dependencies
|
||||
rclcpp
|
||||
nav2_util
|
||||
nav2_map_server
|
||||
nav2_msgs
|
||||
nav_msgs
|
||||
visualization_msgs
|
||||
nav2_amcl
|
||||
nav2_lifecycle_manager
|
||||
nav2_behavior_tree
|
||||
gazebo_ros_pkgs
|
||||
geometry_msgs
|
||||
std_msgs
|
||||
tf2_geometry_msgs
|
||||
rclpy
|
||||
nav2_planner
|
||||
nav2_navfn_planner
|
||||
angles
|
||||
behaviortree_cpp_v3
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
find_package(ament_cmake_pytest REQUIRED)
|
||||
|
||||
add_subdirectory(src/behavior_tree)
|
||||
add_subdirectory(src/planning)
|
||||
add_subdirectory(src/localization)
|
||||
add_subdirectory(src/system)
|
||||
add_subdirectory(src/system_failure)
|
||||
add_subdirectory(src/updown)
|
||||
add_subdirectory(src/waypoint_follower)
|
||||
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/assisted_teleop)
|
||||
add_subdirectory(src/costmap_filters)
|
||||
install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
|
||||
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,14 @@
|
||||
# System Tests
|
||||
|
||||
The package provides tests for Component-Testing, Subsystem-Testing, and Full-System integration. Its main goal is to provide a location for smoke and integration tests of the navigation system to ensure that things are working properly on a high level. Unit and specific subsystem testing happens in the packages specific to those algorithms.
|
||||
|
||||
Most tests in this package will spin up Gazebo instances of a robot in an environment to have the robot complete some task in the space while tracking a specific modules results. Some examples include
|
||||
|
||||
- System tests of a robot in a sandbox environment trying navigate to pose, navigate through poses, and waypoint following navigation types
|
||||
- Random planning of thousands of paths in a generated environment to ensure default planners are working properly
|
||||
- Testing the system can be brought up and down on the lifecycle transitions successfully multiple times
|
||||
- Testing that the keepout and speed restricted zones work in a practical environment without going into keepout zones and slowing in speed restricted areas
|
||||
- Testing behaviors in a sandbox environment to ensure they trigger and complete collision checking properly
|
||||
- Testing system failures are properly recorded and can be recovered from
|
||||
|
||||
This is primarily for use in Nav2 CI to establish a high degree of maintainer confidence when merging in large architectural changes to the Nav2 project. However, this is also useful to test installs of Nav2 locally or for additional information.
|
||||
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 310 B |
@@ -0,0 +1,6 @@
|
||||
image: empty.pgm
|
||||
resolution: 0.1
|
||||
origin: [-10.000000, -10.000000, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
File diff suppressed because one or more lines are too long
@@ -0,0 +1,6 @@
|
||||
image: keepout_mask.pgm
|
||||
resolution: 0.050000
|
||||
origin: [-10.000000, -10.000000, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
@@ -0,0 +1,8 @@
|
||||
image: map_circular.pgm
|
||||
resolution: 0.050000
|
||||
origin: [-10.000000, -10.000000, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
map_type: occupancy
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -0,0 +1,7 @@
|
||||
image: speed_mask.pgm
|
||||
mode: scale
|
||||
resolution: 0.050000
|
||||
origin: [-10.000000, -10.000000, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.99
|
||||
free_thresh: 0.01
|
||||
@@ -0,0 +1,38 @@
|
||||
# Dynamic Obstacle
|
||||
|
||||
Dynamic Obstacle. 30x30x30 cm box.
|
||||
|
||||
Updates its velocity direction and amount every 3 seconds. Max velocity is 0.5.
|
||||
|
||||
## How to Configure a Dynamic Obstacle
|
||||
|
||||
dynamic_obstacle/model.sdf
|
||||
|
||||
<plugin name="random" filename="libRandomVelocityPlugin.so">
|
||||
|
||||
-- Name of the link in this model that receives the velocity
|
||||
<link>link</link>
|
||||
|
||||
-- Initial velocity that is applied to the link
|
||||
<initial_velocity>0 0.05 0</initial_velocity>
|
||||
|
||||
-- Scaling factor that is used to compute a new velocity
|
||||
<velocity_factor>0.5</velocity_factor>
|
||||
|
||||
-- Time delay between new velocities
|
||||
<update_period>3</update_period>
|
||||
|
||||
-- X velocity treshold values
|
||||
<min_x>-0.5</min_x>
|
||||
<max_x>0.5</max_x>
|
||||
|
||||
-- Y velocity treshold values
|
||||
<min_y>-0.5</min_y>
|
||||
<max_y>0.5</max_y>
|
||||
|
||||
-- Z velocity treshold values
|
||||
<min_z>-10</min_z>
|
||||
<max_z>-5</max_z>
|
||||
|
||||
</plugin>
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>DynamicObstacle</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.6">model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Husnu Melih Erdogan</name>
|
||||
<email>h.meliherdogan@gmail.com</email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
Randomly moving obstacle.
|
||||
</description>
|
||||
</model>
|
||||
@@ -0,0 +1,71 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.6">
|
||||
<model name="DynamicObstacle">
|
||||
<pose>0 0 0.15 0 0 0</pose>
|
||||
<link name="link">
|
||||
<gravity>1</gravity>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<inertia>
|
||||
<ixx>0.17</ixx>
|
||||
<ixy>0.00</ixy>
|
||||
<ixz>0.00</ixz>
|
||||
<iyy>0.17</iyy>
|
||||
<iyz>0.00</iyz>
|
||||
<izz>0.17</izz>
|
||||
</inertia>
|
||||
<mass>10</mass>
|
||||
</inertial>
|
||||
<kinematic>false</kinematic>
|
||||
<collision name="collision">
|
||||
<max_contacts>10</max_contacts>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.3 0.3 0.3</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>0.0</mu>
|
||||
<mu2>0.0</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
<bounce>
|
||||
<restitution_coefficient>0.0</restitution_coefficient>
|
||||
<threshold>100000</threshold>
|
||||
</bounce>
|
||||
<contact>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kd>1</kd>
|
||||
<kp>2147483647</kp>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0.0</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.3 0.3 0.3</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<plugin name="random" filename="libRandomVelocityPlugin.so">
|
||||
<link>link</link>
|
||||
<initial_velocity>0 0.05 0</initial_velocity>
|
||||
<velocity_factor>0.5</velocity_factor>
|
||||
<update_period>3</update_period>
|
||||
<min_x>-0.5</min_x>
|
||||
<max_x>0.5</max_x>
|
||||
<min_y>-0.5</min_y>
|
||||
<max_y>0.5</max_y>
|
||||
<min_z>-10</min_z>
|
||||
<max_z>-5</max_z>
|
||||
</plugin>
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -0,0 +1,16 @@
|
||||
# Dynamic World Model
|
||||
|
||||
A world model with a ground plane, 20x20 meter empty room, and 9 dynamic obstacles.
|
||||
|
||||
## How to Add a Dynamic Obstacle
|
||||
|
||||
dynamic_world/world.model
|
||||
|
||||
<model name="DynamicObstacle">
|
||||
-- Position of the new obstacle
|
||||
<pose>-4 4 0.15 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://dynamic_obstacle</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
@@ -0,0 +1,85 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.6">
|
||||
<world name="default">
|
||||
<include>
|
||||
<uri>model://sun</uri>
|
||||
</include>
|
||||
<include>
|
||||
<uri>model://ground_plane</uri>
|
||||
</include>
|
||||
|
||||
<model name="EmptyRoom">
|
||||
<include>
|
||||
<uri>model://empty_room</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
<model name="DynamicObstacle1">
|
||||
<pose>-4 4 0.15 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://dynamic_obstacle</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
<model name="DynamicObstacle2">
|
||||
<pose>-4 0 0.15 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://dynamic_obstacle</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
<model name="DynamicObstacle3">
|
||||
<pose>-4 -4 0.15 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://dynamic_obstacle</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
<model name="DynamicObstacle4">
|
||||
<pose>0 4 0.15 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://dynamic_obstacle</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
<model name="DynamicObstacle5">
|
||||
<pose>0 0 0.15 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://dynamic_obstacle</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
<model name="DynamicObstacle6">
|
||||
<pose>0 -4 0.15 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://dynamic_obstacle</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
<model name="DynamicObstacle7">
|
||||
<pose>4 4 0.15 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://dynamic_obstacle</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
<model name="DynamicObstacle8">
|
||||
<pose>4 0 0.15 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://dynamic_obstacle</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
<model name="DynamicObstacle9">
|
||||
<pose>4 -4 0.15 0 0 0</pose>
|
||||
<include>
|
||||
<uri>model://dynamic_obstacle</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
<include>
|
||||
<pose>2.0 2.0 0.0 0.0 0.0 0.0</pose>
|
||||
<uri>model://turtlebot3_waffle</uri>
|
||||
</include>
|
||||
</world>
|
||||
</sdf>
|
||||
@@ -0,0 +1,3 @@
|
||||
# Empty Room
|
||||
|
||||
20x20 meter empty room.
|
||||
@@ -0,0 +1,11 @@
|
||||
<?xml version="1.0" ?>
|
||||
<model>
|
||||
<name>EmptyRoom</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.6">model.sdf</sdf>
|
||||
<author>
|
||||
<name></name>
|
||||
<email></email>
|
||||
</author>
|
||||
<description></description>
|
||||
</model>
|
||||
@@ -0,0 +1,164 @@
|
||||
<?xml version='1.0'?>
|
||||
<sdf version='1.6'>
|
||||
<model name='EmptyRoom'>
|
||||
<static>1</static>
|
||||
<pose frame=''>0.0 0.0 0.0 0.0 0.0 0.0</pose>
|
||||
<link name='Wall_1'>
|
||||
<collision name='Wall_1_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>10 0.15 1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<max_contacts>1</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
<pose frame=''>0 0 0.5 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_1_Visual'>
|
||||
<pose frame=''>0 0 0.5 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>10 0.15 1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose frame=''>-4.925 0 0 0 -0 -1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_2'>
|
||||
<collision name='Wall_2_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>10 0.15 1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<max_contacts>1</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
<pose frame=''>0 0 0.5 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_2_Visual'>
|
||||
<pose frame=''>0 0 0.5 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>10 0.15 1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose frame=''>-0 -4.925 0 0 -0 0</pose>
|
||||
</link>
|
||||
<link name='Wall_3'>
|
||||
<collision name='Wall_3_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>10 0.15 1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<max_contacts>1</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
<pose frame=''>0 0 0.5 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_3_Visual'>
|
||||
<pose frame=''>0 0 0.5 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>10 0.15 1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose frame=''>4.925 0 0 0 -0 1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_4'>
|
||||
<collision name='Wall_4_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>10 0.15 1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<max_contacts>1</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
<pose frame=''>0 0 0.5 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_4_Visual'>
|
||||
<pose frame=''>0 0 0.5 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>10 0.15 1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose frame=''>-0 4.925 0 0 -0 3.14159</pose>
|
||||
</link>
|
||||
<static>1</static>
|
||||
</model>
|
||||
</sdf>
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@@ -0,0 +1,247 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.4">
|
||||
<model name="turtlebot3_burger">
|
||||
|
||||
<link name="base">
|
||||
|
||||
<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>
|
||||
|
||||
<collision name="caster_collision">
|
||||
<pose>-0.081 0 -0.004 0 0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.005000</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100000.0</mu>
|
||||
<mu2>100000.0</mu2>
|
||||
<fdir1>0 0 0</fdir1>
|
||||
<slip1>0</slip1>
|
||||
<slip2>0</slip2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name="lidar">
|
||||
<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>1</always_on>
|
||||
<visualize>0</visualize>
|
||||
<pose>-0.032 0 0.171 0 0 0</pose>
|
||||
<update_rate>1800</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>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<link name="left_wheel">
|
||||
|
||||
<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="left_wheel_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>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name="left_wheel_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="right_wheel">
|
||||
|
||||
<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="right_wheel_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>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name="right_wheel_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>
|
||||
|
||||
<joint name="left_wheel_joint" type="revolute">
|
||||
<parent>base</parent>
|
||||
<child>left_wheel</child>
|
||||
<pose>0.0 0.08 0.023 -1.57 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="right_wheel_joint" type="revolute">
|
||||
<parent>base</parent>
|
||||
<child>right_wheel</child>
|
||||
<pose>0.0 -0.08 0.023 -1.57 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="lidar_joint" type="fixed">
|
||||
<parent>base</parent>
|
||||
<child>lidar</child>
|
||||
<pose>-0.032 0 0.171 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<use_parent_model_frame>true</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<model>
|
||||
<name>TurtleBot3(Burger)</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.4">model-1_4.sdf</sdf>
|
||||
<sdf version="1.5">model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Taehun Lim(Darby)</name>
|
||||
<email>thlim@robotis.com</email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
TurtleBot3 Burger
|
||||
</description>
|
||||
</model>
|
||||
@@ -0,0 +1,247 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.5">
|
||||
<model name="turtlebot3_burger">
|
||||
|
||||
<link name="base">
|
||||
|
||||
<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>
|
||||
|
||||
<collision name="caster_collision">
|
||||
<pose>-0.081 0 -0.004 0 0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.005000</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100000.0</mu>
|
||||
<mu2>100000.0</mu2>
|
||||
<fdir1>0 0 0</fdir1>
|
||||
<slip1>0</slip1>
|
||||
<slip2>0</slip2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name="lidar">
|
||||
<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>1</always_on>
|
||||
<visualize>0</visualize>
|
||||
<pose>-0.032 0 0.171 0 0 0</pose>
|
||||
<update_rate>1800</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>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<link name="left_wheel">
|
||||
|
||||
<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="left_wheel_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>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name="left_wheel_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="right_wheel">
|
||||
|
||||
<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="right_wheel_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>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name="right_wheel_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>
|
||||
|
||||
<joint name="left_wheel_joint" type="revolute">
|
||||
<parent>base</parent>
|
||||
<child>left_wheel</child>
|
||||
<pose>0.0 0.08 0.023 -1.57 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="right_wheel_joint" type="revolute">
|
||||
<parent>base</parent>
|
||||
<child>right_wheel</child>
|
||||
<pose>0.0 -0.08 0.023 -1.57 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="lidar_joint" type="fixed">
|
||||
<parent>base</parent>
|
||||
<child>lidar</child>
|
||||
<pose>-0.032 0 0.171 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<use_parent_model_frame>true</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
<static>0</static>
|
||||
</model>
|
||||
</sdf>
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@@ -0,0 +1,521 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.4">
|
||||
<model name="turtlebot3_waffle">
|
||||
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
|
||||
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<link name="base_link">
|
||||
|
||||
<inertial>
|
||||
<pose>-0.064 0 0.048 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.064 0 0.048 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.265 0.265 0.089</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name="base_visual">
|
||||
<pose>-0.064 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_waffle/meshes/waffle_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="turtlebot3_imu" filename="libgazebo_ros_imu_sensor.so">
|
||||
<initial_orientation_as_reference>false</initial_orientation_as_reference>
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=imu</remapping>
|
||||
</ros>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<link name="base_scan">
|
||||
<inertial>
|
||||
<pose>-0.052 0 0.111 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.052 0 0.111 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.0508</radius>
|
||||
<length>0.055</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name="lidar_sensor_visual">
|
||||
<pose>-0.064 0 0.121 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_waffle/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>true</visualize>
|
||||
<pose>-0.064 0 0.121 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="turtlebot3_laserscan" filename="libgazebo_ros_ray_sensor.so">
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=scan</remapping>
|
||||
</ros>
|
||||
<output_type>sensor_msgs/LaserScan</output_type>
|
||||
<frame_name>base_scan</frame_name>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<link name="wheel_left_link">
|
||||
|
||||
<inertial>
|
||||
<pose>0.0 0.144 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 0.144 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 0.144 0.023 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_waffle/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.144 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.144 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.144 0.023 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_waffle/meshes/right_tire.dae</uri>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name='caster_back_right_link'>
|
||||
<pose>-0.177 -0.064 -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>
|
||||
|
||||
<link name='caster_back_left_link'>
|
||||
<pose>-0.177 0.064 -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>
|
||||
|
||||
<link name="camera_link"/>
|
||||
|
||||
<link name="camera_rgb_frame">
|
||||
<inertial>
|
||||
<pose>0.069 -0.047 0.107 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.035</mass>
|
||||
</inertial>
|
||||
|
||||
<collision name="collision">
|
||||
<pose>0 0 0.047 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.008 0.130 0.022</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<pose>0.069 -0.047 0.107 0 0 0</pose>
|
||||
<sensor name="camera" type="camera">
|
||||
<always_on>true</always_on>
|
||||
<visualize>true</visualize>
|
||||
<update_rate>30</update_rate>
|
||||
<camera name="intel_realsense_r200">
|
||||
<horizontal_fov>1.02974</horizontal_fov>
|
||||
<image>
|
||||
<width>1920</width>
|
||||
<height>1080</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.02</near>
|
||||
<far>300</far>
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<!-- Noise is sampled independently per pixel on each frame.
|
||||
That pixel's noise value is added to each of its color
|
||||
channels, which at that point lie in the range [0,1]. -->
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.007</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
<plugin name="camera_driver" filename="libgazebo_ros_camera.so">
|
||||
<ros>
|
||||
<!-- <namespace>test_cam</namespace> -->
|
||||
<!-- <remapping>image_raw:=image_demo</remapping> -->
|
||||
<!-- <remapping>camera_info:=camera_info_demo</remapping> -->
|
||||
</ros>
|
||||
<!-- camera_name>omit so it defaults to sensor name</camera_name-->
|
||||
<!-- frame_name>omit so it defaults to link name</frameName-->
|
||||
<!-- <hack_baseline>0.07</hack_baseline> -->
|
||||
</plugin>
|
||||
</sensor>
|
||||
</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.144 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.144 0.023 -1.57 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name='caster_back_right_joint' type='ball'>
|
||||
<parent>base_link</parent>
|
||||
<child>caster_back_right_link</child>
|
||||
</joint>
|
||||
|
||||
<joint name='caster_back_left_joint' type='ball'>
|
||||
<parent>base_link</parent>
|
||||
<child>caster_back_left_link</child>
|
||||
</joint>
|
||||
|
||||
<joint name="lidar_joint" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>base_scan</child>
|
||||
<pose>-0.064 0 0.121 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>camera_link</child>
|
||||
<pose>0.064 -0.065 0.094 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="camera_rgb_joint" type="fixed">
|
||||
<parent>camera_link</parent>
|
||||
<child>camera_rgb_frame</child>
|
||||
<pose>0.005 0.018 0.013 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> -->
|
||||
<remapping>/tf:=tf</remapping>
|
||||
</ros>
|
||||
|
||||
<update_rate>30</update_rate>
|
||||
|
||||
<!-- wheels -->
|
||||
<left_joint>wheel_left_joint</left_joint>
|
||||
<right_joint>wheel_right_joint</right_joint>
|
||||
|
||||
<!-- kinematics -->
|
||||
<wheel_separation>0.287</wheel_separation>
|
||||
<wheel_diameter>0.066</wheel_diameter>
|
||||
|
||||
<!-- limits -->
|
||||
<max_wheel_torque>20</max_wheel_torque>
|
||||
<max_wheel_acceleration>1.0</max_wheel_acceleration>
|
||||
|
||||
<command_topic>cmd_vel</command_topic>
|
||||
|
||||
<!-- output -->
|
||||
<publish_odom>true</publish_odom>
|
||||
<publish_odom_tf>true</publish_odom_tf>
|
||||
<publish_wheel_tf>false</publish_wheel_tf>
|
||||
|
||||
<odometry_topic>odom</odometry_topic>
|
||||
<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> -->
|
||||
<remapping>~/out:=joint_states</remapping>
|
||||
</ros>
|
||||
<update_rate>30</update_rate>
|
||||
<joint_name>wheel_left_joint</joint_name>
|
||||
<joint_name>wheel_right_joint</joint_name>
|
||||
</plugin>
|
||||
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<model>
|
||||
<name>TurtleBot3(Waffle)</name>
|
||||
<version>2.0</version>
|
||||
<sdf version="1.4">model-1_4.sdf</sdf>
|
||||
<sdf version="1.5">model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Taehun Lim(Darby)</name>
|
||||
<email>thlim@robotis.com</email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
TurtleBot3 Waffle
|
||||
</description>
|
||||
</model>
|
||||
@@ -0,0 +1,521 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.5">
|
||||
<model name="turtlebot3_waffle">
|
||||
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
|
||||
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<link name="base_link">
|
||||
|
||||
<inertial>
|
||||
<pose>-0.064 0 0.048 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.064 0 0.048 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.265 0.265 0.089</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name="base_visual">
|
||||
<pose>-0.064 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_waffle/meshes/waffle_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="turtlebot3_imu" filename="libgazebo_ros_imu_sensor.so">
|
||||
<initial_orientation_as_reference>false</initial_orientation_as_reference>
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=imu</remapping>
|
||||
</ros>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<link name="base_scan">
|
||||
<inertial>
|
||||
<pose>-0.052 0 0.111 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.052 0 0.111 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.0508</radius>
|
||||
<length>0.055</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name="lidar_sensor_visual">
|
||||
<pose>-0.064 0 0.121 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_waffle/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>true</visualize>
|
||||
<pose>-0.064 0 0.121 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="turtlebot3_laserscan" filename="libgazebo_ros_ray_sensor.so">
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=scan</remapping>
|
||||
</ros>
|
||||
<output_type>sensor_msgs/LaserScan</output_type>
|
||||
<frame_name>base_scan</frame_name>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<link name="wheel_left_link">
|
||||
|
||||
<inertial>
|
||||
<pose>0.0 0.144 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 0.144 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 0.144 0.023 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_waffle/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.144 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.144 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.144 0.023 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_waffle/meshes/right_tire.dae</uri>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name='caster_back_right_link'>
|
||||
<pose>-0.177 -0.064 -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>
|
||||
|
||||
<link name='caster_back_left_link'>
|
||||
<pose>-0.177 0.064 -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>
|
||||
|
||||
<link name="camera_link"/>
|
||||
|
||||
<link name="camera_rgb_frame">
|
||||
<inertial>
|
||||
<pose>0.069 -0.047 0.107 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.035</mass>
|
||||
</inertial>
|
||||
|
||||
<collision name="collision">
|
||||
<pose>0 0.047 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.008 0.130 0.022</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<pose>0.069 -0.047 0.107 0 0 0</pose>
|
||||
<sensor name="camera" type="camera">
|
||||
<always_on>true</always_on>
|
||||
<visualize>true</visualize>
|
||||
<update_rate>30</update_rate>
|
||||
<camera name="intel_realsense_r200">
|
||||
<horizontal_fov>1.02974</horizontal_fov>
|
||||
<image>
|
||||
<width>1920</width>
|
||||
<height>1080</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.02</near>
|
||||
<far>300</far>
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<!-- Noise is sampled independently per pixel on each frame.
|
||||
That pixel's noise value is added to each of its color
|
||||
channels, which at that point lie in the range [0,1]. -->
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.007</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
<plugin name="camera_driver" filename="libgazebo_ros_camera.so">
|
||||
<ros>
|
||||
<!-- <namespace>test_cam</namespace> -->
|
||||
<!-- <remapping>image_raw:=image_demo</remapping> -->
|
||||
<!-- <remapping>camera_info:=camera_info_demo</remapping> -->
|
||||
</ros>
|
||||
<!-- camera_name>omit so it defaults to sensor name</camera_name-->
|
||||
<!-- frame_name>omit so it defaults to link name</frameName-->
|
||||
<!-- <hack_baseline>0.07</hack_baseline> -->
|
||||
</plugin>
|
||||
</sensor>
|
||||
</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.144 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.144 0.023 -1.57 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name='caster_back_right_joint' type='ball'>
|
||||
<parent>base_link</parent>
|
||||
<child>caster_back_right_link</child>
|
||||
</joint>
|
||||
|
||||
<joint name='caster_back_left_joint' type='ball'>
|
||||
<parent>base_link</parent>
|
||||
<child>caster_back_left_link</child>
|
||||
</joint>
|
||||
|
||||
<joint name="lidar_joint" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>base_scan</child>
|
||||
<pose>-0.064 0 0.121 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>camera_link</child>
|
||||
<pose>0.064 -0.065 0.094 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="camera_rgb_joint" type="fixed">
|
||||
<parent>camera_link</parent>
|
||||
<child>camera_rgb_frame</child>
|
||||
<pose>0.005 0.018 0.013 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> -->
|
||||
<remapping>/tf:=tf</remapping>
|
||||
</ros>
|
||||
|
||||
<update_rate>30</update_rate>
|
||||
|
||||
<!-- wheels -->
|
||||
<left_joint>wheel_left_joint</left_joint>
|
||||
<right_joint>wheel_right_joint</right_joint>
|
||||
|
||||
<!-- kinematics -->
|
||||
<wheel_separation>0.287</wheel_separation>
|
||||
<wheel_diameter>0.066</wheel_diameter>
|
||||
|
||||
<!-- limits -->
|
||||
<max_wheel_torque>20</max_wheel_torque>
|
||||
<max_wheel_acceleration>1.0</max_wheel_acceleration>
|
||||
|
||||
<command_topic>cmd_vel</command_topic>
|
||||
|
||||
<!-- output -->
|
||||
<publish_odom>true</publish_odom>
|
||||
<publish_odom_tf>true</publish_odom_tf>
|
||||
<publish_wheel_tf>false</publish_wheel_tf>
|
||||
|
||||
<odometry_topic>odom</odometry_topic>
|
||||
<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> -->
|
||||
<remapping>~/out:=joint_states</remapping>
|
||||
</ros>
|
||||
<update_rate>30</update_rate>
|
||||
<joint_name>wheel_left_joint</joint_name>
|
||||
<joint_name>wheel_right_joint</joint_name>
|
||||
</plugin>
|
||||
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -0,0 +1,19 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<model>
|
||||
<name>TurtleBot3_Waffle_DepthCamera</name>
|
||||
<version>2.0</version>
|
||||
<sdf version="1.5">model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Taehun Lim(Darby)</name>
|
||||
<email>thlim@robotis.com</email>
|
||||
|
||||
<name>Melih Erdogan(mlherd)</name>
|
||||
<email>h.meliherdogan@gmail.com</email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
TurtleBot3 Waffle with Depth Camera
|
||||
</description>
|
||||
</model>
|
||||
@@ -0,0 +1,454 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.5">
|
||||
<model name="TurtleBot3_Waffle_DepthCamera">
|
||||
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
|
||||
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<link name="base_link">
|
||||
|
||||
<inertial>
|
||||
<pose>-0.064 0 0.048 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.064 0 0.048 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.265 0.265 0.089</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
</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="turtlebot3_imu" filename="libgazebo_ros_imu_sensor.so">
|
||||
<initial_orientation_as_reference>false</initial_orientation_as_reference>
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=imu</remapping>
|
||||
</ros>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<link name="base_scan">
|
||||
<inertial>
|
||||
<pose>-0.052 0 0.111 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.052 0 0.111 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.0508</radius>
|
||||
<length>0.055</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<sensor name="hls_lfcd_lds" type="ray">
|
||||
<always_on>true</always_on>
|
||||
<visualize>true</visualize>
|
||||
<pose>-0.064 0 0.121 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="turtlebot3_laserscan" filename="libgazebo_ros_ray_sensor.so">
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=scan</remapping>
|
||||
</ros>
|
||||
<output_type>sensor_msgs/LaserScan</output_type>
|
||||
<frame_name>base_scan</frame_name>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<link name="wheel_left_link">
|
||||
|
||||
<inertial>
|
||||
<pose>0.0 0.144 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 0.144 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>
|
||||
</link>
|
||||
|
||||
<link name="wheel_right_link">
|
||||
|
||||
<inertial>
|
||||
<pose>0.0 -0.144 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.144 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>
|
||||
</link>
|
||||
|
||||
<link name='caster_back_right_link'>
|
||||
<pose>-0.177 -0.064 -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>
|
||||
|
||||
<link name='caster_back_left_link'>
|
||||
<pose>-0.177 0.064 -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>
|
||||
|
||||
<link name="camera_link">
|
||||
<inertial>
|
||||
<pose>0.069 -0.047 0.107 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.035</mass>
|
||||
</inertial>
|
||||
|
||||
<collision name="collision">
|
||||
<pose>0 0.047 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.008 0.130 0.022</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<pose>0.069 -0.047 0.107 0 0 0</pose>
|
||||
|
||||
<sensor name="intel_realsense_r200_depth" type="depth">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>15</update_rate>
|
||||
<pose>0.064 -0.047 0.107 0 0 0</pose>
|
||||
<camera name="realsense_depth_camera">
|
||||
</camera>
|
||||
<plugin name="intel_realsense_r200_depth_driver" filename="libgazebo_ros_camera.so">
|
||||
<ros>
|
||||
<!--
|
||||
<remapping>custom_camera/image_raw:=custom_camera/custom_image</remapping>
|
||||
<remapping>custom_camera/image_depth:=custom_camera/custom_image_depth</remapping>
|
||||
<remapping>custom_camera/camera_info:=custom_camera/custom_info_raw</remapping>
|
||||
<remapping>custom_camera/camera_info_depth:=custom_camera/custom_info_depth</remapping>
|
||||
<remapping>custom_camera/points:=custom_camera/custom_points</remapping>
|
||||
-->
|
||||
</ros>
|
||||
<camera_name>intel_realsense_r200_depth</camera_name>
|
||||
<frame_name>camera_depth_frame</frame_name>
|
||||
<hack_baseline>0.07</hack_baseline>
|
||||
<min_depth>0.001</min_depth>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</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.144 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.144 0.023 -1.57 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name='caster_back_right_joint' type='ball'>
|
||||
<parent>base_link</parent>
|
||||
<child>caster_back_right_link</child>
|
||||
</joint>
|
||||
|
||||
<joint name='caster_back_left_joint' type='ball'>
|
||||
<parent>base_link</parent>
|
||||
<child>caster_back_left_link</child>
|
||||
</joint>
|
||||
|
||||
<joint name="lidar_joint" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>base_scan</child>
|
||||
<pose>-0.064 0 0.121 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>camera_link</child>
|
||||
</joint>
|
||||
|
||||
<plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so">
|
||||
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>/tf:=tf</remapping>
|
||||
</ros>
|
||||
|
||||
<update_rate>30</update_rate>
|
||||
|
||||
<!-- wheels -->
|
||||
<left_joint>wheel_left_joint</left_joint>
|
||||
<right_joint>wheel_right_joint</right_joint>
|
||||
|
||||
<!-- kinematics -->
|
||||
<wheel_separation>0.287</wheel_separation>
|
||||
<wheel_diameter>0.066</wheel_diameter>
|
||||
|
||||
<!-- limits -->
|
||||
<max_wheel_torque>20</max_wheel_torque>
|
||||
<max_wheel_acceleration>1.0</max_wheel_acceleration>
|
||||
|
||||
<command_topic>cmd_vel</command_topic>
|
||||
|
||||
<!-- output -->
|
||||
<publish_odom>true</publish_odom>
|
||||
<publish_odom_tf>true</publish_odom_tf>
|
||||
<publish_wheel_tf>false</publish_wheel_tf>
|
||||
|
||||
<odometry_topic>odom</odometry_topic>
|
||||
<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> -->
|
||||
<remapping>~/out:=joint_states</remapping>
|
||||
</ros>
|
||||
<update_rate>30</update_rate>
|
||||
<joint_name>wheel_left_joint</joint_name>
|
||||
<joint_name>wheel_right_joint</joint_name>
|
||||
</plugin>
|
||||
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -0,0 +1,76 @@
|
||||
<COLLADA version="1.4.1" xmlns="http://www.collada.org/2005/11/COLLADASchema">
|
||||
<asset>
|
||||
<created>2017-03-30T01:06:40</created>
|
||||
<modified>2017-03-30T01:06:40</modified>
|
||||
<unit name="inch" meter="0.0254"/>
|
||||
<up_axis>Y_UP</up_axis>
|
||||
</asset>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="ID1">
|
||||
<node>
|
||||
<instance_geometry url="#ID2">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="Material0" target="#ID9"/>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
</node>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<library_geometries>
|
||||
<geometry id="ID2">
|
||||
<mesh>
|
||||
<source id="ID3">
|
||||
<float_array id="ID4" count="108">-28.8675 50 100 -57.735 9.66338e-13 0 -57.735 1.13687e-12 100 -28.8675 50 0 -57.735 1.13687e-12 100 -28.8675 -50 0 -28.8675 -50 100 -57.735 9.66338e-13 0 -28.8675 -50 100 28.8675 -50 0 28.8675 -50 100 -28.8675 -50 0 28.8675 -50 100 57.735 1.7053e-12 0 57.735 1.36424e-12 100 28.8675 -50 0 57.735 1.36424e-12 100 28.8675 50 2.498e-13 28.8675 50 100 57.735 1.7053e-12 0 28.8675 50 100 -28.8675 50 0 -28.8675 50 100 28.8675 50 2.498e-13 -57.735 1.13687e-12 100 57.735 1.36424e-12 100 -28.8675 50 100 -28.8675 -50 100 28.8675 -50 100 28.8675 50 100 -57.735 9.66338e-13 0 28.8675 -50 0 -28.8675 -50 0 57.735 1.7053e-12 0 -28.8675 50 0 28.8675 50 2.498e-13</float_array>
|
||||
<technique_common>
|
||||
<accessor stride="3" count="36" source="#ID4">
|
||||
<param type="float" name="X"/>
|
||||
<param type="float" name="Y"/>
|
||||
<param type="float" name="Z"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="ID5">
|
||||
<float_array id="ID6" count="108">-0.866025 0.5 0 -0.866025 0.5 0 -0.866025 0.5 0 -0.866025 0.5 0 -0.866025 -0.5 0 -0.866025 -0.5 0 -0.866025 -0.5 0 -0.866025 -0.5 0 5.51091e-16 -1 0 5.51091e-16 -1 0 5.51091e-16 -1 0 5.51091e-16 -1 0 0.866025 -0.5 0 0.866025 -0.5 0 0.866025 -0.5 0 0.866025 -0.5 0 0.866025 0.5 0 0.866025 0.5 0 0.866025 0.5 0 0.866025 0.5 0 -4.28626e-16 1 0 -4.28626e-16 1 0 -4.28626e-16 1 0 -4.28626e-16 1 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1</float_array>
|
||||
<technique_common>
|
||||
<accessor stride="3" count="36" source="#ID6">
|
||||
<param type="float" name="X"/>
|
||||
<param type="float" name="Y"/>
|
||||
<param type="float" name="Z"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="ID7">
|
||||
<input semantic="POSITION" source="#ID3"/>
|
||||
<input semantic="NORMAL" source="#ID5"/>
|
||||
</vertices>
|
||||
<triangles count="20" material="Material0">
|
||||
<input semantic="VERTEX" source="#ID7" offset="0"/>
|
||||
<p>0 1 2 1 0 3 4 5 6 5 4 7 8 9 10 9 8 11 12 13 14 13 12 15 16 17 18 17 16 19 20 21 22 21 20 23 24 25 26 24 27 28 28 25 24 26 25 29 30 31 32 31 30 33 33 34 35 33 30 34</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_materials>
|
||||
<material id="ID9">
|
||||
<instance_effect url="#ID8"/>
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_effects>
|
||||
<effect id="ID8">
|
||||
<profile_COMMON>
|
||||
<technique sid="COMMON">
|
||||
<lambert>
|
||||
<diffuse>
|
||||
<color>0.392157 0.976471 0.121569 1</color>
|
||||
</diffuse>
|
||||
</lambert>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<scene>
|
||||
<instance_visual_scene url="#ID1"/>
|
||||
</scene>
|
||||
</COLLADA>
|
||||
@@ -0,0 +1,76 @@
|
||||
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
|
||||
<asset>
|
||||
<created>2017-03-30T02:35:53</created>
|
||||
<modified>2017-03-30T02:35:53</modified>
|
||||
<unit name="inch" meter="0.0254"/>
|
||||
<up_axis>Y_UP</up_axis>
|
||||
</asset>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="ID1">
|
||||
<node>
|
||||
<instance_geometry url="#ID2">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="Material0" target="#ID9"/>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
</node>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<library_geometries>
|
||||
<geometry id="ID2">
|
||||
<mesh>
|
||||
<source id="ID3">
|
||||
<float_array count="216" id="ID4">-400 230.94 0 -400 230.94 200 4.54747e-13 461.88 200 6.25278e-13 461.88 0 6.25278e-13 461.88 0 4.54747e-13 461.88 200 400 230.94 200 400 230.94 0 400 230.94 0 400 230.94 200 400 -230.94 200 400 -230.94 0 400 -230.94 0 400 -230.94 200 -1.7053e-13 -461.88 200 2.27374e-13 -461.88 0 2.27374e-13 -461.88 0 -1.7053e-13 -461.88 200 -400 -230.94 200 -400 -230.94 0 450 259.808 200 -3.69482e-13 519.615 200 -2.27374e-13 519.615 0 450 259.808 0 -3.69482e-13 519.615 200 -450 259.808 200 -450 259.808 0 -2.27374e-13 519.615 0 -450 259.808 200 -450 -259.808 200 -450 -259.808 0 -450 259.808 0 -450 -259.808 200 -1.13687e-13 -519.615 200 -2.27374e-13 -519.615 0 -450 -259.808 0 -1.13687e-13 -519.615 200 450 -259.808 200 450 -259.808 0 -2.27374e-13 -519.615 0 450 -259.808 0 450 259.808 200 450 259.808 0 450 -259.808 200 -400 -230.94 0 -400 -230.94 200 -400 230.94 200 -400 230.94 0 -1.7053e-13 -461.88 200 -1.13687e-13 -519.615 200 -400 -230.94 200 -450 -259.808 200 -450 259.808 200 450 -259.808 200 400 -230.94 200 400 230.94 200 4.54747e-13 461.88 200 -3.69482e-13 519.615 200 -400 230.94 200 450 259.808 200 -2.27374e-13 -519.615 0 400 -230.94 0 2.27374e-13 -461.88 0 400 230.94 0 -2.27374e-13 519.615 0 6.25278e-13 461.88 0 450 -259.808 0 450 259.808 0 -400 230.94 0 -450 259.808 0 -400 -230.94 0 -450 -259.808 0</float_array>
|
||||
<technique_common>
|
||||
<accessor count="72" source="#ID4" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="ID5">
|
||||
<float_array count="216" id="ID6">0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 -0.5 -0.866025 -0 -0.5 -0.866025 -0 -0.5 -0.866025 -0 -0.5 -0.866025 -0 -1 -6.12323e-17 -0 -1 -6.12323e-17 -0 -1 -6.12323e-17 -0 -1 -6.12323e-17 -0 -0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 -1 -5.21642e-16 0 -1 -5.21642e-16 0 -1 -5.21642e-16 0 -1 -5.21642e-16 0 -0.5 -0.866025 0 -0.5 -0.866025 0 -0.5 -0.866025 0 -0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 1 0 0 1 0 0 1 0 0 1 0 0 1 1.83697e-16 0 1 1.83697e-16 0 1 1.83697e-16 0 1 1.83697e-16 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1</float_array>
|
||||
<technique_common>
|
||||
<accessor count="72" source="#ID6" stride="3">
|
||||
<param name="X" type="float"/>
|
||||
<param name="Y" type="float"/>
|
||||
<param name="Z" type="float"/>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="ID7">
|
||||
<input semantic="POSITION" source="#ID3"/>
|
||||
<input semantic="NORMAL" source="#ID5"/>
|
||||
</vertices>
|
||||
<triangles count="48" material="Material0">
|
||||
<input semantic="VERTEX" source="#ID7" offset="0"/>
|
||||
<p>0 2 1 0 3 2 4 6 5 4 7 6 8 10 9 8 11 10 12 14 13 12 15 14 16 18 17 16 19 18 20 22 21 22 20 23 24 26 25 26 24 27 28 30 29 30 28 31 32 34 33 34 32 35 36 38 37 38 36 39 40 42 41 40 41 43 44 46 45 44 47 46 48 50 49 50 52 51 53 54 49 55 57 56 48 49 54 57 52 58 58 56 57 53 59 55 57 55 59 53 55 54 49 50 51 52 50 58 60 62 61 63 65 64 63 67 66 68 69 64 66 61 63 64 67 63 60 61 66 60 70 62 69 70 71 60 71 70 64 65 68 69 68 70</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_materials>
|
||||
<material id="ID9">
|
||||
<instance_effect url="#ID8"/>
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_effects>
|
||||
<effect id="ID8">
|
||||
<profile_COMMON>
|
||||
<technique sid="COMMON">
|
||||
<lambert>
|
||||
<diffuse>
|
||||
<color>0.603922 0.647059 0.686275 1</color>
|
||||
</diffuse>
|
||||
</lambert>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<scene>
|
||||
<instance_visual_scene url="#ID1"/>
|
||||
</scene>
|
||||
</COLLADA>
|
||||
@@ -0,0 +1,549 @@
|
||||
<sdf version='1.4'>
|
||||
<!-- Draw Circle -->
|
||||
<model name='ros_symbol'>
|
||||
<static>1</static>
|
||||
<link name='symbol'>
|
||||
<collision name='one_one'>
|
||||
<pose>-1.1 -1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='one_one'>
|
||||
<pose>-1.1 -1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='one_two'>
|
||||
<pose>-1.1 0 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='one_two'>
|
||||
<pose>-1.1 0 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='one_three'>
|
||||
<pose>-1.1 1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='one_three'>
|
||||
<pose>-1.1 1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='two_one'>
|
||||
<pose>0 -1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='two_one'>
|
||||
<pose>0 -1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='two_two'>
|
||||
<pose>0 0 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='two_two'>
|
||||
<pose>0 0 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='two_three'>
|
||||
<pose>0 1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='two_three'>
|
||||
<pose>0 1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='three_one'>
|
||||
<pose>1.1 -1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='three_one'>
|
||||
<pose>1.1 -1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='three_two'>
|
||||
<pose>1.1 0 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='three_two'>
|
||||
<pose>1.1 0 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='three_three'>
|
||||
<pose>1.1 1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='three_three'>
|
||||
<pose>1.1 1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<!-- Draw Hexagon -->
|
||||
<collision name='head'>
|
||||
<pose>3.5 0 -0.5 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.8 0.8 0.8</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='head'>
|
||||
<pose>3.5 0 -0.5 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.8 0.8 0.8</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Green</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='left_hand'>
|
||||
<pose>1.8 2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='left_hand'>
|
||||
<pose>1.8 2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Green</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='right_hand'>
|
||||
<pose>1.8 -2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='right_hand'>
|
||||
<pose>1.8 -2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Green</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='left_foot'>
|
||||
<pose>-1.8 2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='left_foot'>
|
||||
<pose>-1.8 2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Green</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='right_foot'>
|
||||
<pose>-1.8 -2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='right_foot'>
|
||||
<pose>-1.8 -2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Green</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<!-- Draw Wall -->
|
||||
<collision name='body'>
|
||||
<pose>0 0 -0.3 0 0 -1.5708</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/wall.dae</uri>
|
||||
<scale>0.25 0.25 0.25</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='body'>
|
||||
<pose>0 0 -0.3 0 0 -1.5708</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/wall.dae</uri>
|
||||
<scale>0.25 0.25 0.25</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/FlatBlack</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<model>
|
||||
<name>TurtleBot3 World</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.4">model-1_4.sdf</sdf>
|
||||
<sdf version="1.5">model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Taehun Lim(Darby)</name>
|
||||
<email>thlim@robotis.com</email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
World of TurtleBot3 with ROS symbol
|
||||
</description>
|
||||
</model>
|
||||
@@ -0,0 +1,549 @@
|
||||
<sdf version='1.5'>
|
||||
<!-- Draw Circle -->
|
||||
<model name='ros_symbol'>
|
||||
<static>1</static>
|
||||
<link name='symbol'>
|
||||
<collision name='one_one'>
|
||||
<pose>-1.1 -1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='one_one'>
|
||||
<pose>-1.1 -1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='one_two'>
|
||||
<pose>-1.1 0 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='one_two'>
|
||||
<pose>-1.1 0 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='one_three'>
|
||||
<pose>-1.1 1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='one_three'>
|
||||
<pose>-1.1 1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='two_one'>
|
||||
<pose>0 -1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='two_one'>
|
||||
<pose>0 -1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='two_two'>
|
||||
<pose>0 0 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='two_two'>
|
||||
<pose>0 0 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='two_three'>
|
||||
<pose>0 1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='two_three'>
|
||||
<pose>0 1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='three_one'>
|
||||
<pose>1.1 -1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='three_one'>
|
||||
<pose>1.1 -1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='three_two'>
|
||||
<pose>1.1 0 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='three_two'>
|
||||
<pose>1.1 0 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='three_three'>
|
||||
<pose>1.1 1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='three_three'>
|
||||
<pose>1.1 1.1 0.25 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.15</radius>
|
||||
<length>0.5</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/White</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<!-- Draw Hexagon -->
|
||||
<collision name='head'>
|
||||
<pose>3.5 0 -0.5 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.8 0.8 0.8</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='head'>
|
||||
<pose>3.5 0 -0.5 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.8 0.8 0.8</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Green</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='left_hand'>
|
||||
<pose>1.8 2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='left_hand'>
|
||||
<pose>1.8 2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Green</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='right_hand'>
|
||||
<pose>1.8 -2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='right_hand'>
|
||||
<pose>1.8 -2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Green</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='left_foot'>
|
||||
<pose>-1.8 2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='left_foot'>
|
||||
<pose>-1.8 2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Green</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision name='right_foot'>
|
||||
<pose>-1.8 -2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='right_foot'>
|
||||
<pose>-1.8 -2.7 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/hexagon.dae</uri>
|
||||
<scale>0.55 0.55 0.55</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Green</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<!-- Draw Wall -->
|
||||
<collision name='body'>
|
||||
<pose>0 0 -0.3 0 0 -1.5708</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/wall.dae</uri>
|
||||
<scale>0.25 0.25 0.25</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<bounce/>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name='body'>
|
||||
<pose>0 0 -0.3 0 0 -1.5708</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_world/meshes/wall.dae</uri>
|
||||
<scale>0.25 0.25 0.25</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/FlatBlack</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
@@ -0,0 +1,69 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>nav2_system_tests</name>
|
||||
<version>1.1.18</version>
|
||||
<description>TODO</description>
|
||||
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>nav2_common</build_depend>
|
||||
|
||||
<build_depend>rclcpp</build_depend>
|
||||
<build_depend>rclpy</build_depend>
|
||||
<build_depend>nav2_util</build_depend>
|
||||
<build_depend>nav2_map_server</build_depend>
|
||||
<build_depend>nav2_msgs</build_depend>
|
||||
<build_depend>nav2_lifecycle_manager</build_depend>
|
||||
<build_depend>nav2_navfn_planner</build_depend>
|
||||
<build_depend>nav2_behavior_tree</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>visualization_msgs</build_depend>
|
||||
<build_depend>nav2_amcl</build_depend>
|
||||
<build_depend>launch_ros</build_depend>
|
||||
<build_depend>launch_testing</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>tf2_geometry_msgs</build_depend>
|
||||
<build_depend>gazebo_ros_pkgs</build_depend>
|
||||
<build_depend>launch_ros</build_depend>
|
||||
<build_depend>launch_testing</build_depend>
|
||||
<build_depend>nav2_planner</build_depend>
|
||||
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>launch_testing</exec_depend>
|
||||
<exec_depend>rclcpp</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>nav2_bringup</exec_depend>
|
||||
<exec_depend>nav2_util</exec_depend>
|
||||
<exec_depend>nav2_map_server</exec_depend>
|
||||
<exec_depend>nav2_msgs</exec_depend>
|
||||
<exec_depend>nav2_lifecycle_manager</exec_depend>
|
||||
<exec_depend>nav2_navfn_planner</exec_depend>
|
||||
<exec_depend>nav2_behavior_tree</exec_depend>
|
||||
<exec_depend>nav_msgs</exec_depend>
|
||||
<exec_depend>visualization_msgs</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>nav2_amcl</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>tf2_geometry_msgs</exec_depend>
|
||||
<exec_depend>gazebo_ros_pkgs</exec_depend>
|
||||
<exec_depend>navigation2</exec_depend>
|
||||
<exec_depend>lcov</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>nav2_planner</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>ament_cmake_pytest</test_depend>
|
||||
<test_depend>launch</test_depend>
|
||||
<test_depend>launch_ros</test_depend>
|
||||
<test_depend>launch_testing</test_depend>
|
||||
<test_depend>python3-zmq</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
+50
@@ -0,0 +1,50 @@
|
||||
#!/bin/bash
|
||||
|
||||
#
|
||||
# Simple bash script to loop over the Nav2 "bt_navigator" system test
|
||||
#
|
||||
# options:
|
||||
# -c <#> - number of times to loop
|
||||
# -o <file> - name of summary output file
|
||||
# -l <file> - name to use for failing log files
|
||||
# -d <dds> - name of DDS implementation to use (ex: rmw_fastrtps_cpp)
|
||||
|
||||
failcount=0
|
||||
loopcount=1
|
||||
|
||||
while getopts c:o:l:d: option
|
||||
do
|
||||
case "$option"
|
||||
in
|
||||
c) loopcount=$OPTARG;;
|
||||
o) outfile=$OPTARG;;
|
||||
l) logfile=$OPTARG;;
|
||||
d) dds=$OPTARG;;
|
||||
esac
|
||||
done
|
||||
|
||||
echo "Total loop count = " $loopcount
|
||||
export RMW_IMPLEMENTATION=$dds
|
||||
|
||||
for ((i=1; i<=$loopcount; i++))
|
||||
do
|
||||
echo "******************************"
|
||||
echo "Loop number: " $i
|
||||
echo "Running with DDS: " $dds
|
||||
echo "******************************"
|
||||
|
||||
ctest -V -R test_bt_navigator$ -O $logfile
|
||||
result=$?
|
||||
echo "RESULT =" $result
|
||||
if [ "$result" != "0" ]
|
||||
then
|
||||
((failcount+=1))
|
||||
echo "TEST $i FAILED" >> $outfile
|
||||
mv $logfile $logfile.$i.fail
|
||||
fi
|
||||
echo $i "TESTS COMPLETED"
|
||||
echo $failcount "TOTAL FAILURES"
|
||||
done
|
||||
echo $failcount " FAILURES / " $loopcount
|
||||
echo $failcount " FAILURES / " $loopcount >> $outfile
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
find_package(Boost COMPONENTS system filesystem REQUIRED)
|
||||
|
||||
ament_add_gtest(test_behavior_tree_node
|
||||
test_behavior_tree_node.cpp
|
||||
server_handler.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(test_behavior_tree_node
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
target_include_directories(test_behavior_tree_node PUBLIC ${Boost_INCLUDE_DIRS})
|
||||
target_link_libraries(test_behavior_tree_node
|
||||
${Boost_FILESYSTEM_LIBRARY}
|
||||
${Boost_SYSTEM_LIBRARY}
|
||||
)
|
||||
@@ -0,0 +1,207 @@
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef BEHAVIOR_TREE__DUMMY_SERVERS_HPP_
|
||||
#define BEHAVIOR_TREE__DUMMY_SERVERS_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
#include <chrono>
|
||||
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
using namespace std::chrono_literals; // NOLINT
|
||||
using namespace std::chrono; // NOLINT
|
||||
using namespace std::placeholders; // NOLINT
|
||||
|
||||
template<class ServiceT>
|
||||
class DummyService
|
||||
{
|
||||
public:
|
||||
explicit DummyService(
|
||||
const rclcpp::Node::SharedPtr & node,
|
||||
std::string service_name)
|
||||
: node_(node),
|
||||
service_name_(service_name),
|
||||
request_count_(0),
|
||||
disabled_(false)
|
||||
{
|
||||
server_ = node->create_service<ServiceT>(
|
||||
service_name,
|
||||
std::bind(&DummyService::handle_service, this, _1, _2, _3));
|
||||
}
|
||||
|
||||
void disable()
|
||||
{
|
||||
server_.reset();
|
||||
disabled_ = true;
|
||||
}
|
||||
|
||||
void enable()
|
||||
{
|
||||
if (disabled_) {
|
||||
server_ = node_->create_service<ServiceT>(
|
||||
service_name_,
|
||||
std::bind(&DummyService::handle_service, this, _1, _2, _3));
|
||||
disabled_ = false;
|
||||
}
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
enable();
|
||||
request_count_ = 0;
|
||||
}
|
||||
|
||||
int getRequestCount() const
|
||||
{
|
||||
return request_count_;
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void fillResponse(
|
||||
const std::shared_ptr<typename ServiceT::Request>/*request*/,
|
||||
const std::shared_ptr<typename ServiceT::Response>/*response*/) {}
|
||||
|
||||
void handle_service(
|
||||
const std::shared_ptr<rmw_request_id_t>/*request_header*/,
|
||||
const std::shared_ptr<typename ServiceT::Request> request,
|
||||
const std::shared_ptr<typename ServiceT::Response> response)
|
||||
{
|
||||
request_count_++;
|
||||
fillResponse(request, response);
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
typename rclcpp::Service<ServiceT>::SharedPtr server_;
|
||||
std::string service_name_;
|
||||
int request_count_;
|
||||
bool disabled_;
|
||||
};
|
||||
|
||||
template<class ActionT>
|
||||
class DummyActionServer
|
||||
{
|
||||
public:
|
||||
explicit DummyActionServer(
|
||||
const rclcpp::Node::SharedPtr & node,
|
||||
std::string action_name)
|
||||
: action_name_(action_name),
|
||||
goal_count_(0)
|
||||
{
|
||||
this->action_server_ = rclcpp_action::create_server<ActionT>(
|
||||
node->get_node_base_interface(),
|
||||
node->get_node_clock_interface(),
|
||||
node->get_node_logging_interface(),
|
||||
node->get_node_waitables_interface(),
|
||||
action_name,
|
||||
std::bind(&DummyActionServer::handle_goal, this, _1, _2),
|
||||
std::bind(&DummyActionServer::handle_cancel, this, _1),
|
||||
std::bind(&DummyActionServer::handle_accepted, this, _1));
|
||||
}
|
||||
|
||||
void setFailureRanges(const std::vector<std::pair<int, int>> & failureRanges)
|
||||
{
|
||||
failure_ranges_ = failureRanges;
|
||||
}
|
||||
|
||||
void setRunningRanges(const std::vector<std::pair<int, int>> & runningRanges)
|
||||
{
|
||||
running_ranges_ = runningRanges;
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
failure_ranges_.clear();
|
||||
running_ranges_.clear();
|
||||
goal_count_ = 0;
|
||||
}
|
||||
|
||||
int getGoalCount() const
|
||||
{
|
||||
return goal_count_;
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual std::shared_ptr<typename ActionT::Result> fillResult()
|
||||
{
|
||||
return std::make_shared<typename ActionT::Result>();
|
||||
}
|
||||
|
||||
virtual rclcpp_action::GoalResponse handle_goal(
|
||||
const rclcpp_action::GoalUUID &,
|
||||
std::shared_ptr<const typename ActionT::Goal>/*goal*/)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
virtual rclcpp_action::CancelResponse handle_cancel(
|
||||
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>)
|
||||
{
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void execute(
|
||||
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle)
|
||||
{
|
||||
goal_count_++;
|
||||
auto result = fillResult();
|
||||
|
||||
// if current goal index exists in running range, the thread sleeps for 1 second
|
||||
// to simulate a long running action
|
||||
for (auto & index : running_ranges_) {
|
||||
if (goal_count_ >= index.first && goal_count_ <= index.second) {
|
||||
std::this_thread::sleep_for(1s);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// if current goal index exists in failure range, the goal will be aborted
|
||||
for (auto & index : failure_ranges_) {
|
||||
if (goal_count_ >= index.first && goal_count_ <= index.second) {
|
||||
goal_handle->abort(result);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// goal succeeds for all other indices
|
||||
goal_handle->succeed(result);
|
||||
}
|
||||
|
||||
void handle_accepted(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle)
|
||||
{
|
||||
using namespace std::placeholders; // NOLINT
|
||||
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
|
||||
std::thread{std::bind(&DummyActionServer::execute, this, _1), goal_handle}.detach();
|
||||
}
|
||||
|
||||
protected:
|
||||
typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
|
||||
std::string action_name_;
|
||||
|
||||
// contains pairs of indices which define a range for which the
|
||||
// requested action goal will return running for 1s or be aborted
|
||||
// for all other indices, the action server will return success
|
||||
std::vector<std::pair<int, int>> failure_ranges_;
|
||||
std::vector<std::pair<int, int>> running_ranges_;
|
||||
|
||||
int goal_count_;
|
||||
};
|
||||
|
||||
#endif // BEHAVIOR_TREE__DUMMY_SERVERS_HPP_
|
||||
@@ -0,0 +1,96 @@
|
||||
// Copyright (c) 2020 Vinny Ruia
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
|
||||
#include "server_handler.hpp"
|
||||
|
||||
using namespace std::chrono_literals; // NOLINT
|
||||
using namespace std::chrono; // NOLINT
|
||||
|
||||
ServerHandler::ServerHandler()
|
||||
: is_active_(false)
|
||||
{
|
||||
node_ = rclcpp::Node::make_shared("behavior_tree_tester");
|
||||
|
||||
clear_local_costmap_server = std::make_unique<DummyService<nav2_msgs::srv::ClearEntireCostmap>>(
|
||||
node_, "local_costmap/clear_entirely_local_costmap");
|
||||
clear_global_costmap_server = std::make_unique<DummyService<nav2_msgs::srv::ClearEntireCostmap>>(
|
||||
node_, "global_costmap/clear_entirely_global_costmap");
|
||||
compute_path_to_pose_server = std::make_unique<ComputePathToPoseActionServer>(node_);
|
||||
follow_path_server = std::make_unique<DummyActionServer<nav2_msgs::action::FollowPath>>(
|
||||
node_, "follow_path");
|
||||
spin_server = std::make_unique<DummyActionServer<nav2_msgs::action::Spin>>(
|
||||
node_, "spin");
|
||||
wait_server = std::make_unique<DummyActionServer<nav2_msgs::action::Wait>>(
|
||||
node_, "wait");
|
||||
backup_server = std::make_unique<DummyActionServer<nav2_msgs::action::BackUp>>(
|
||||
node_, "backup");
|
||||
drive_on_heading_server = std::make_unique<DummyActionServer<nav2_msgs::action::DriveOnHeading>>(
|
||||
node_, "drive_on_heading");
|
||||
ntp_server = std::make_unique<DummyActionServer<nav2_msgs::action::ComputePathThroughPoses>>(
|
||||
node_, "compute_path_through_poses");
|
||||
}
|
||||
|
||||
ServerHandler::~ServerHandler()
|
||||
{
|
||||
if (is_active_) {
|
||||
deactivate();
|
||||
}
|
||||
}
|
||||
|
||||
void ServerHandler::activate()
|
||||
{
|
||||
if (is_active_) {
|
||||
throw std::runtime_error("Trying to activate while already activated");
|
||||
}
|
||||
|
||||
is_active_ = true;
|
||||
server_thread_ =
|
||||
std::make_shared<std::thread>(std::bind(&ServerHandler::spinThread, this));
|
||||
|
||||
std::cout << "Server handler is active!" << std::endl;
|
||||
}
|
||||
|
||||
void ServerHandler::deactivate()
|
||||
{
|
||||
if (!is_active_) {
|
||||
throw std::runtime_error("Trying to deactivate while already inactive");
|
||||
}
|
||||
|
||||
is_active_ = false;
|
||||
server_thread_->join();
|
||||
|
||||
std::cout << "Server handler has been deactivated!" << std::endl;
|
||||
}
|
||||
|
||||
void ServerHandler::reset() const
|
||||
{
|
||||
clear_global_costmap_server->reset();
|
||||
clear_local_costmap_server->reset();
|
||||
compute_path_to_pose_server->reset();
|
||||
follow_path_server->reset();
|
||||
spin_server->reset();
|
||||
wait_server->reset();
|
||||
backup_server->reset();
|
||||
drive_on_heading_server->reset();
|
||||
}
|
||||
|
||||
void ServerHandler::spinThread()
|
||||
{
|
||||
rclcpp::spin(node_);
|
||||
}
|
||||
@@ -0,0 +1,108 @@
|
||||
// Copyright (c) 2020 Vinny Ruia
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef BEHAVIOR_TREE__SERVER_HANDLER_HPP_
|
||||
#define BEHAVIOR_TREE__SERVER_HANDLER_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
|
||||
#include "nav2_msgs/action/compute_path_to_pose.hpp"
|
||||
#include "nav2_msgs/action/follow_path.hpp"
|
||||
#include "nav2_msgs/action/spin.hpp"
|
||||
#include "nav2_msgs/action/back_up.hpp"
|
||||
#include "nav2_msgs/action/wait.hpp"
|
||||
#include "nav2_msgs/action/drive_on_heading.hpp"
|
||||
#include "nav2_msgs/action/compute_path_through_poses.hpp"
|
||||
|
||||
#include "geometry_msgs/msg/point_stamped.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "dummy_servers.hpp"
|
||||
|
||||
class ComputePathToPoseActionServer
|
||||
: public DummyActionServer<nav2_msgs::action::ComputePathToPose>
|
||||
{
|
||||
public:
|
||||
explicit ComputePathToPoseActionServer(const rclcpp::Node::SharedPtr & node)
|
||||
: DummyActionServer(node, "compute_path_to_pose")
|
||||
{
|
||||
result_ = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>();
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header = result_->path.header;
|
||||
pose.pose.position.x = 0.0;
|
||||
pose.pose.position.y = 0.0;
|
||||
pose.pose.position.z = 0.0;
|
||||
pose.pose.orientation.x = 0.0;
|
||||
pose.pose.orientation.y = 0.0;
|
||||
pose.pose.orientation.z = 0.0;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
result_->path.poses.push_back(pose);
|
||||
}
|
||||
}
|
||||
|
||||
std::shared_ptr<nav2_msgs::action::ComputePathToPose::Result> fillResult() override
|
||||
{
|
||||
return result_;
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<nav2_msgs::action::ComputePathToPose::Result> result_;
|
||||
};
|
||||
|
||||
class ServerHandler
|
||||
{
|
||||
public:
|
||||
ServerHandler();
|
||||
~ServerHandler();
|
||||
|
||||
void activate();
|
||||
|
||||
void deactivate();
|
||||
|
||||
bool isActive() const
|
||||
{
|
||||
return is_active_;
|
||||
}
|
||||
|
||||
void reset() const;
|
||||
|
||||
public:
|
||||
std::unique_ptr<DummyService<nav2_msgs::srv::ClearEntireCostmap>> clear_local_costmap_server;
|
||||
std::unique_ptr<DummyService<nav2_msgs::srv::ClearEntireCostmap>> clear_global_costmap_server;
|
||||
std::unique_ptr<ComputePathToPoseActionServer> compute_path_to_pose_server;
|
||||
std::unique_ptr<DummyActionServer<nav2_msgs::action::FollowPath>> follow_path_server;
|
||||
std::unique_ptr<DummyActionServer<nav2_msgs::action::Spin>> spin_server;
|
||||
std::unique_ptr<DummyActionServer<nav2_msgs::action::Wait>> wait_server;
|
||||
std::unique_ptr<DummyActionServer<nav2_msgs::action::BackUp>> backup_server;
|
||||
std::unique_ptr<DummyActionServer<nav2_msgs::action::DriveOnHeading>> drive_on_heading_server;
|
||||
std::unique_ptr<DummyActionServer<nav2_msgs::action::ComputePathThroughPoses>> ntp_server;
|
||||
|
||||
private:
|
||||
void spinThread();
|
||||
|
||||
bool is_active_;
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
std::shared_ptr<std::thread> server_thread_;
|
||||
};
|
||||
|
||||
#endif // BEHAVIOR_TREE__SERVER_HANDLER_HPP_
|
||||
@@ -0,0 +1,656 @@
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <fstream>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <boost/filesystem.hpp>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
#include "behaviortree_cpp_v3/behavior_tree.h"
|
||||
#include "behaviortree_cpp_v3/bt_factory.h"
|
||||
#include "behaviortree_cpp_v3/utils/shared_library.h"
|
||||
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
#include "tf2_ros/create_timer_ros.h"
|
||||
|
||||
#include "nav2_util/odometry_utils.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "ament_index_cpp/get_package_share_directory.hpp"
|
||||
|
||||
#include "server_handler.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
namespace fs = boost::filesystem;
|
||||
|
||||
class BehaviorTreeHandler
|
||||
{
|
||||
public:
|
||||
BehaviorTreeHandler()
|
||||
{
|
||||
node_ = rclcpp::Node::make_shared("behavior_tree_handler");
|
||||
|
||||
tf_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
|
||||
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
|
||||
node_->get_node_base_interface(), node_->get_node_timers_interface());
|
||||
tf_->setCreateTimerInterface(timer_interface);
|
||||
tf_->setUsingDedicatedThread(true);
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, node_, false);
|
||||
|
||||
odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(node_);
|
||||
|
||||
const std::vector<std::string> plugin_libs = {
|
||||
"nav2_compute_path_to_pose_action_bt_node",
|
||||
"nav2_compute_path_through_poses_action_bt_node",
|
||||
"nav2_smooth_path_action_bt_node",
|
||||
"nav2_follow_path_action_bt_node",
|
||||
"nav2_spin_action_bt_node",
|
||||
"nav2_wait_action_bt_node",
|
||||
"nav2_assisted_teleop_action_bt_node",
|
||||
"nav2_back_up_action_bt_node",
|
||||
"nav2_drive_on_heading_bt_node",
|
||||
"nav2_clear_costmap_service_bt_node",
|
||||
"nav2_is_stuck_condition_bt_node",
|
||||
"nav2_goal_reached_condition_bt_node",
|
||||
"nav2_initial_pose_received_condition_bt_node",
|
||||
"nav2_goal_updated_condition_bt_node",
|
||||
"nav2_globally_updated_goal_condition_bt_node",
|
||||
"nav2_is_path_valid_condition_bt_node",
|
||||
"nav2_reinitialize_global_localization_service_bt_node",
|
||||
"nav2_rate_controller_bt_node",
|
||||
"nav2_distance_controller_bt_node",
|
||||
"nav2_speed_controller_bt_node",
|
||||
"nav2_truncate_path_action_bt_node",
|
||||
"nav2_truncate_path_local_action_bt_node",
|
||||
"nav2_goal_updater_node_bt_node",
|
||||
"nav2_recovery_node_bt_node",
|
||||
"nav2_pipeline_sequence_bt_node",
|
||||
"nav2_round_robin_node_bt_node",
|
||||
"nav2_transform_available_condition_bt_node",
|
||||
"nav2_time_expired_condition_bt_node",
|
||||
"nav2_path_expiring_timer_condition",
|
||||
"nav2_distance_traveled_condition_bt_node",
|
||||
"nav2_single_trigger_bt_node",
|
||||
"nav2_is_battery_low_condition_bt_node",
|
||||
"nav2_navigate_through_poses_action_bt_node",
|
||||
"nav2_navigate_to_pose_action_bt_node",
|
||||
"nav2_remove_passed_goals_action_bt_node",
|
||||
"nav2_planner_selector_bt_node",
|
||||
"nav2_controller_selector_bt_node",
|
||||
"nav2_goal_checker_selector_bt_node",
|
||||
"nav2_controller_cancel_bt_node",
|
||||
"nav2_path_longer_on_approach_bt_node",
|
||||
"nav2_assisted_teleop_cancel_bt_node",
|
||||
"nav2_wait_cancel_bt_node",
|
||||
"nav2_spin_cancel_bt_node",
|
||||
"nav2_back_up_cancel_bt_node",
|
||||
"nav2_drive_on_heading_cancel_bt_node",
|
||||
"nav2_goal_updated_controller_bt_node"
|
||||
};
|
||||
for (const auto & p : plugin_libs) {
|
||||
factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p));
|
||||
}
|
||||
}
|
||||
|
||||
bool loadBehaviorTree(const std::string & filename)
|
||||
{
|
||||
// Read the input BT XML from the specified file into a string
|
||||
std::ifstream xml_file(filename);
|
||||
|
||||
if (!xml_file.good()) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Couldn't open input XML file: %s", filename.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
auto xml_string = std::string(
|
||||
std::istreambuf_iterator<char>(xml_file),
|
||||
std::istreambuf_iterator<char>());
|
||||
|
||||
// Create the blackboard that will be shared by all of the nodes in the tree
|
||||
blackboard = BT::Blackboard::create();
|
||||
|
||||
// Put items on the blackboard
|
||||
blackboard->set<rclcpp::Node::SharedPtr>("node", node_); // NOLINT
|
||||
blackboard->set<std::chrono::milliseconds>(
|
||||
"server_timeout", std::chrono::milliseconds(20)); // NOLINT
|
||||
blackboard->set<std::chrono::milliseconds>(
|
||||
"bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT
|
||||
blackboard->set<std::chrono::milliseconds>(
|
||||
"wait_for_service_timeout", std::chrono::milliseconds(1000)); // NOLINT
|
||||
blackboard->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", tf_); // NOLINT
|
||||
blackboard->set<bool>("initial_pose_received", false); // NOLINT
|
||||
blackboard->set<int>("number_recoveries", 0); // NOLINT
|
||||
blackboard->set<std::shared_ptr<nav2_util::OdomSmoother>>("odom_smoother", odom_smoother_); // NOLINT
|
||||
|
||||
// set dummy goal on blackboard
|
||||
geometry_msgs::msg::PoseStamped goal;
|
||||
goal.header.stamp = node_->now();
|
||||
goal.header.frame_id = "map";
|
||||
goal.pose.position.x = 0.0;
|
||||
goal.pose.position.y = 0.0;
|
||||
goal.pose.position.z = 0.0;
|
||||
goal.pose.orientation.x = 0.0;
|
||||
goal.pose.orientation.y = 0.0;
|
||||
goal.pose.orientation.z = 0.0;
|
||||
goal.pose.orientation.w = 1.0;
|
||||
|
||||
blackboard->set<geometry_msgs::msg::PoseStamped>("goal", goal); // NOLINT
|
||||
|
||||
// Create the Behavior Tree from the XML input
|
||||
try {
|
||||
tree = factory_.createTreeFromText(xml_string, blackboard);
|
||||
} catch (BT::RuntimeError & exp) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "%s: %s", filename.c_str(), exp.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public:
|
||||
BT::Blackboard::Ptr blackboard;
|
||||
BT::Tree tree;
|
||||
|
||||
private:
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
|
||||
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
|
||||
|
||||
BT::BehaviorTreeFactory factory_;
|
||||
};
|
||||
|
||||
class BehaviorTreeTestFixture : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
static void SetUpTestCase()
|
||||
{
|
||||
// initialize ROS
|
||||
rclcpp::init(0, nullptr);
|
||||
|
||||
server_handler = std::make_shared<ServerHandler>();
|
||||
if (!server_handler->isActive()) {
|
||||
server_handler->activate();
|
||||
}
|
||||
}
|
||||
|
||||
static void TearDownTestCase()
|
||||
{
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
server_handler.reset();
|
||||
bt_handler.reset();
|
||||
}
|
||||
|
||||
void SetUp() override
|
||||
{
|
||||
server_handler->reset();
|
||||
bt_handler = std::make_shared<BehaviorTreeHandler>();
|
||||
}
|
||||
|
||||
void TearDown() override
|
||||
{
|
||||
bt_handler.reset();
|
||||
}
|
||||
|
||||
protected:
|
||||
static std::shared_ptr<ServerHandler> server_handler;
|
||||
static std::shared_ptr<BehaviorTreeHandler> bt_handler;
|
||||
};
|
||||
|
||||
std::shared_ptr<ServerHandler> BehaviorTreeTestFixture::server_handler = nullptr;
|
||||
std::shared_ptr<BehaviorTreeHandler> BehaviorTreeTestFixture::bt_handler = nullptr;
|
||||
|
||||
TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles)
|
||||
{
|
||||
fs::path root = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
|
||||
root /= "behavior_trees/";
|
||||
|
||||
if (boost::filesystem::exists(root) && boost::filesystem::is_directory(root)) {
|
||||
for (auto const & entry : boost::filesystem::recursive_directory_iterator(root)) {
|
||||
if (boost::filesystem::is_regular_file(entry) && entry.path().extension() == ".xml") {
|
||||
std::cout << entry.path().string() << std::endl;
|
||||
EXPECT_EQ(bt_handler->loadBehaviorTree(entry.path().string()), true);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Test scenario:
|
||||
*
|
||||
* ComputePathToPose and FollowPath return SUCCESS
|
||||
* The behavior tree should execute correctly and return SUCCESS
|
||||
*/
|
||||
TEST_F(BehaviorTreeTestFixture, TestAllSuccess)
|
||||
{
|
||||
// Load behavior tree from file
|
||||
fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
|
||||
bt_file /= "behavior_trees/";
|
||||
bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
|
||||
EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
|
||||
|
||||
BT::NodeStatus result = BT::NodeStatus::RUNNING;
|
||||
|
||||
while (result == BT::NodeStatus::RUNNING) {
|
||||
result = bt_handler->tree.tickRoot();
|
||||
std::this_thread::sleep_for(10ms);
|
||||
}
|
||||
|
||||
// The final result should be success since all action servers returned success
|
||||
EXPECT_EQ(result, BT::NodeStatus::SUCCESS);
|
||||
|
||||
// Goal count should be 1 since only one goal is sent to ComputePathToPose and FollowPath servers
|
||||
EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 1);
|
||||
EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 1);
|
||||
|
||||
// Goal count should be 0 since no goal is sent to all other servers
|
||||
EXPECT_EQ(server_handler->spin_server->getGoalCount(), 0);
|
||||
EXPECT_EQ(server_handler->wait_server->getGoalCount(), 0);
|
||||
EXPECT_EQ(server_handler->backup_server->getGoalCount(), 0);
|
||||
EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 0);
|
||||
EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Test scenario:
|
||||
*
|
||||
* ComputePathToPose returns FAILURE and ClearGlobalCostmap-Context returns FAILURE
|
||||
* PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback
|
||||
* GoalUpdated returns FAILURE and RoundRobin is triggered
|
||||
* RoundRobin triggers ClearingActions Sequence which returns FAILURE
|
||||
* RoundRobin triggers Spin, Wait, and BackUp which return FAILURE
|
||||
* RoundRobin returns FAILURE hence RecoveryCallbackk returns FAILURE
|
||||
* Finally NavigateRecovery returns FAILURE
|
||||
* The behavior tree should also return FAILURE
|
||||
*/
|
||||
TEST_F(BehaviorTreeTestFixture, TestAllFailure)
|
||||
{
|
||||
// Load behavior tree from file
|
||||
fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
|
||||
bt_file /= "behavior_trees/";
|
||||
bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
|
||||
EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
|
||||
|
||||
// Set all action server to fail the first 100 times
|
||||
std::vector<std::pair<int, int>> failureRange;
|
||||
failureRange.emplace_back(std::pair<int, int>(0, 100));
|
||||
server_handler->compute_path_to_pose_server->setFailureRanges(failureRange);
|
||||
server_handler->follow_path_server->setFailureRanges(failureRange);
|
||||
server_handler->spin_server->setFailureRanges(failureRange);
|
||||
server_handler->wait_server->setFailureRanges(failureRange);
|
||||
server_handler->backup_server->setFailureRanges(failureRange);
|
||||
|
||||
// Disable services
|
||||
server_handler->clear_global_costmap_server->disable();
|
||||
server_handler->clear_local_costmap_server->disable();
|
||||
|
||||
BT::NodeStatus result = BT::NodeStatus::RUNNING;
|
||||
|
||||
while (result == BT::NodeStatus::RUNNING) {
|
||||
result = bt_handler->tree.tickRoot();
|
||||
std::this_thread::sleep_for(10ms);
|
||||
}
|
||||
|
||||
// The final result should be failure
|
||||
EXPECT_EQ(result, BT::NodeStatus::FAILURE);
|
||||
|
||||
// Goal count should be 1 since only one goal is sent to ComputePathToPose
|
||||
EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 1);
|
||||
|
||||
// Goal count should be 0 since no goal is sent to FollowPath action server
|
||||
EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 0);
|
||||
|
||||
// All recovery action servers were sent 1 goal
|
||||
EXPECT_EQ(server_handler->spin_server->getGoalCount(), 1);
|
||||
EXPECT_EQ(server_handler->wait_server->getGoalCount(), 1);
|
||||
EXPECT_EQ(server_handler->backup_server->getGoalCount(), 1);
|
||||
|
||||
// Service count is 0 since the server was disabled
|
||||
EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 0);
|
||||
EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Test scenario:
|
||||
*
|
||||
* ComputePathToPose returns FAILURE on the first try triggering the planner recovery
|
||||
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns SUCCESS when retried
|
||||
* FollowPath returns FAILURE on the first try triggering the controller recovery
|
||||
* ClearLocalCostmap-Context returns SUCCESS and FollowPath returns SUCCESS when retried
|
||||
* The behavior tree should return SUCCESS
|
||||
*/
|
||||
TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries)
|
||||
{
|
||||
// Load behavior tree from file
|
||||
fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
|
||||
bt_file /= "behavior_trees/";
|
||||
bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
|
||||
EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
|
||||
|
||||
// Set ComputePathToPose and FollowPath action servers to fail for the first action
|
||||
std::vector<std::pair<int, int>> failureRange;
|
||||
failureRange.emplace_back(std::pair<int, int>(0, 1));
|
||||
server_handler->compute_path_to_pose_server->setFailureRanges(failureRange);
|
||||
server_handler->follow_path_server->setFailureRanges(failureRange);
|
||||
|
||||
BT::NodeStatus result = BT::NodeStatus::RUNNING;
|
||||
|
||||
while (result == BT::NodeStatus::RUNNING) {
|
||||
result = bt_handler->tree.tickRoot();
|
||||
std::this_thread::sleep_for(10ms);
|
||||
}
|
||||
|
||||
// The final result should be success
|
||||
EXPECT_EQ(result, BT::NodeStatus::SUCCESS);
|
||||
|
||||
// Goal count should be 2 since only two goals were sent to ComputePathToPose and FollowPath
|
||||
EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 2);
|
||||
EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 2);
|
||||
|
||||
// Navigate subtree recovery services are called once each
|
||||
EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 1);
|
||||
EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 1);
|
||||
|
||||
// Goal count should be 0 since no goal is sent to all other servers
|
||||
EXPECT_EQ(server_handler->spin_server->getGoalCount(), 0);
|
||||
EXPECT_EQ(server_handler->wait_server->getGoalCount(), 0);
|
||||
EXPECT_EQ(server_handler->backup_server->getGoalCount(), 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Test scenario:
|
||||
*
|
||||
* ComputePathToPose returns FAILURE on the first try triggering the planner recovery
|
||||
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns SUCCESS when retried
|
||||
* FollowPath returns FAILURE on the first try triggering the controller recovery
|
||||
* ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
|
||||
* FollowPath returns FAILURE again and PipelineSequence returns FAILURE
|
||||
* NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE
|
||||
* RoundRobin triggers ClearingActions Sequence which returns SUCCESS
|
||||
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
|
||||
* PipelineSequence is triggered again and ComputePathToPose returns SUCCESS
|
||||
* FollowPath returns FAILURE on the third try triggering the controller recovery
|
||||
* ClearLocalCostmap-Context returns SUCCESS and FollowPath returns SUCCESS on the fourth try
|
||||
* The behavior tree should return SUCCESS
|
||||
*/
|
||||
TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple)
|
||||
{
|
||||
// Load behavior tree from file
|
||||
fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
|
||||
bt_file /= "behavior_trees/";
|
||||
bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
|
||||
EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
|
||||
|
||||
// Set ComputePathToPose action server to fail for the first action
|
||||
std::vector<std::pair<int, int>> plannerFailureRange;
|
||||
plannerFailureRange.emplace_back(std::pair<int, int>(0, 1));
|
||||
server_handler->compute_path_to_pose_server->setFailureRanges(plannerFailureRange);
|
||||
|
||||
// Set FollowPath action server to fail for the first 3 actions
|
||||
std::vector<std::pair<int, int>> controllerFailureRange;
|
||||
controllerFailureRange.emplace_back(std::pair<int, int>(0, 3));
|
||||
server_handler->follow_path_server->setFailureRanges(controllerFailureRange);
|
||||
|
||||
BT::NodeStatus result = BT::NodeStatus::RUNNING;
|
||||
|
||||
while (result == BT::NodeStatus::RUNNING) {
|
||||
result = bt_handler->tree.tickRoot();
|
||||
std::this_thread::sleep_for(10ms);
|
||||
}
|
||||
|
||||
// The final result should be success
|
||||
EXPECT_EQ(result, BT::NodeStatus::SUCCESS);
|
||||
|
||||
// FollowPath is called 4 times
|
||||
EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 4);
|
||||
|
||||
// ComputePathToPose is called 3 times
|
||||
EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 3);
|
||||
|
||||
// Local costmap is cleared 3 times
|
||||
EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 3);
|
||||
|
||||
// Global costmap is cleared 2 times
|
||||
EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 2);
|
||||
|
||||
// Goal count should be 0 since only no goal is sent to all other servers
|
||||
EXPECT_EQ(server_handler->spin_server->getGoalCount(), 0);
|
||||
EXPECT_EQ(server_handler->wait_server->getGoalCount(), 0);
|
||||
EXPECT_EQ(server_handler->backup_server->getGoalCount(), 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Test scenario:
|
||||
*
|
||||
* ComputePathToPose returns FAILURE on the first try triggering the planner recovery
|
||||
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
|
||||
* PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback
|
||||
* GoalUpdated returns FAILURE, RoundRobin triggers ClearingActions Sequence which returns SUCCESS
|
||||
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
|
||||
*
|
||||
* PipelineSequence is triggered again and ComputePathToPose returns SUCCESS (retry #1)
|
||||
* FollowPath returns FAILURE on the first try triggering the controller recovery
|
||||
* ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
|
||||
* FollowPath returns FAILURE again and PipelineSequence returns FAILURE
|
||||
* NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE
|
||||
* RoundRobin triggers Spin which returns FAILURE
|
||||
* RoundRobin triggers Wait which returns SUCCESS
|
||||
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
|
||||
*
|
||||
* PipelineSequence is triggered again and ComputePathToPose returns FAILURE (retry #2)
|
||||
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
|
||||
* PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback
|
||||
* GoalUpdated returns FAILURE and RoundRobin triggers BackUp which returns FAILURE
|
||||
* RoundRobin triggers ClearingActions Sequence which returns SUCCESS
|
||||
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
|
||||
*
|
||||
* PipelineSequence is triggered again and ComputePathToPose returns FAILURE (retry #3)
|
||||
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
|
||||
* PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback
|
||||
* GoalUpdated returns FAILURE and RoundRobin triggers Spin which returns SUCCESS
|
||||
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
|
||||
*
|
||||
* PipelineSequence is triggered again and ComputePathToPose returns FAILURE (retry #4)
|
||||
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
|
||||
* PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback
|
||||
* GoalUpdated returns FAILURE and RoundRobin triggers Wait which returns FAILURE
|
||||
* RoundRobin triggers BackUp which returns SUCCESS
|
||||
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
|
||||
*
|
||||
* PipelineSequence is triggered again and ComputePathToPose returns SUCCESS (retry #5)
|
||||
* FollowPath returns FAILURE on the first try triggering the controller recovery
|
||||
* ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
|
||||
* FollowPath returns FAILURE again and PipelineSequence returns FAILURE
|
||||
* NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE
|
||||
* RoundRobin triggers ClearingActions Sequence which returns SUCCESS
|
||||
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
|
||||
*
|
||||
* PipelineSequence is triggered again and ComputePathToPose returns FAILURE (retry #6)
|
||||
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
|
||||
* PipelineSequence returns FAILURE and NavigateRecovery finally also returns FAILURE
|
||||
*
|
||||
* The behavior tree should return FAILURE
|
||||
*/
|
||||
TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex)
|
||||
{
|
||||
// Load behavior tree from file
|
||||
fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
|
||||
bt_file /= "behavior_trees/";
|
||||
bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
|
||||
EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
|
||||
|
||||
// Set ComputePathToPose action server to fail for the first 2 actions
|
||||
std::vector<std::pair<int, int>> plannerFailureRange;
|
||||
plannerFailureRange.emplace_back(std::pair<int, int>(0, 2));
|
||||
plannerFailureRange.emplace_back(std::pair<int, int>(4, 9));
|
||||
plannerFailureRange.emplace_back(std::pair<int, int>(11, 12));
|
||||
server_handler->compute_path_to_pose_server->setFailureRanges(plannerFailureRange);
|
||||
|
||||
// Set FollowPath action server to fail for the first 2 actions
|
||||
std::vector<std::pair<int, int>> controllerFailureRange;
|
||||
controllerFailureRange.emplace_back(std::pair<int, int>(0, 4));
|
||||
server_handler->follow_path_server->setFailureRanges(controllerFailureRange);
|
||||
|
||||
// Set Spin action server to fail for the first action
|
||||
std::vector<std::pair<int, int>> spinFailureRange;
|
||||
spinFailureRange.emplace_back(std::pair<int, int>(0, 1));
|
||||
server_handler->spin_server->setFailureRanges(spinFailureRange);
|
||||
|
||||
// Set Wait action server to fail for the first action
|
||||
std::vector<std::pair<int, int>> waitFailureRange;
|
||||
waitFailureRange.emplace_back(std::pair<int, int>(2, 2));
|
||||
server_handler->wait_server->setFailureRanges(waitFailureRange);
|
||||
|
||||
// Set BackUp action server to fail for the first action
|
||||
std::vector<std::pair<int, int>> backupFailureRange;
|
||||
backupFailureRange.emplace_back(std::pair<int, int>(0, 1));
|
||||
server_handler->backup_server->setFailureRanges(backupFailureRange);
|
||||
|
||||
BT::NodeStatus result = BT::NodeStatus::RUNNING;
|
||||
|
||||
while (result == BT::NodeStatus::RUNNING) {
|
||||
result = bt_handler->tree.tickRoot();
|
||||
std::this_thread::sleep_for(10ms);
|
||||
}
|
||||
|
||||
// The final result should be success
|
||||
EXPECT_EQ(result, BT::NodeStatus::FAILURE);
|
||||
|
||||
// ComputePathToPose is called 12 times
|
||||
EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 12);
|
||||
|
||||
// FollowPath is called 4 times
|
||||
EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 4);
|
||||
|
||||
// Local costmap is cleared 5 times
|
||||
EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 5);
|
||||
|
||||
// Global costmap is cleared 8 times
|
||||
EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 8);
|
||||
|
||||
// All recovery action servers receive 2 goals
|
||||
EXPECT_EQ(server_handler->spin_server->getGoalCount(), 2);
|
||||
EXPECT_EQ(server_handler->wait_server->getGoalCount(), 2);
|
||||
EXPECT_EQ(server_handler->backup_server->getGoalCount(), 2);
|
||||
}
|
||||
|
||||
/**
|
||||
* Test scenario:
|
||||
*
|
||||
* ComputePathToPose returns FAILURE on the first try triggering the planner recovery
|
||||
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
|
||||
* PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback
|
||||
* GoalUpdated returns FAILURE, RoundRobin triggers ClearingActions Sequence which returns SUCCESS
|
||||
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
|
||||
* PipelineSequence is triggered again and ComputePathToPose returns SUCCESS
|
||||
* FollowPath returns FAILURE on the first try triggering the controller recovery
|
||||
* ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
|
||||
* FollowPath returns FAILURE and PipelineSequence returns FAILURE
|
||||
* NavigateRecovery triggers RecoveryFallback which triggers GoalUpdated
|
||||
* GoalUpdated returns FAILURE and RecoveryFallback triggers RoundRobin
|
||||
* RoundRobin triggers Spin which returns RUNNING
|
||||
*
|
||||
* At this point a new goal is updated on the blackboard
|
||||
*
|
||||
* RecoveryFallback triggers GoalUpdated which returns SUCCESS this time
|
||||
* Since GoalUpdated returned SUCCESS, RoundRobin and hence Spin is halted
|
||||
* RecoveryFallback also returns SUCCESS and PipelineSequence is retried
|
||||
* PipelineSequence triggers ComputePathToPose which returns SUCCESS
|
||||
* FollowPath returns SUCCESS and NavigateRecovery finally also returns SUCCESS
|
||||
*
|
||||
* The behavior tree should return SUCCESS
|
||||
*/
|
||||
TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated)
|
||||
{
|
||||
// Load behavior tree from file
|
||||
fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
|
||||
bt_file /= "behavior_trees/";
|
||||
bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
|
||||
EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
|
||||
|
||||
// Set ComputePathToPose action server to fail for the first 2 actions
|
||||
std::vector<std::pair<int, int>> plannerFailureRange;
|
||||
plannerFailureRange.emplace_back(std::pair<int, int>(0, 2));
|
||||
server_handler->compute_path_to_pose_server->setFailureRanges(plannerFailureRange);
|
||||
|
||||
// Set FollowPath action server to fail for the first 2 actions
|
||||
std::vector<std::pair<int, int>> controllerFailureRange;
|
||||
controllerFailureRange.emplace_back(std::pair<int, int>(0, 2));
|
||||
server_handler->follow_path_server->setFailureRanges(controllerFailureRange);
|
||||
|
||||
// Set Spin action server to return running for the first action
|
||||
std::vector<std::pair<int, int>> spinRunningRange;
|
||||
spinRunningRange.emplace_back(std::pair<int, int>(1, 1));
|
||||
server_handler->spin_server->setRunningRanges(spinRunningRange);
|
||||
|
||||
BT::NodeStatus result = BT::NodeStatus::RUNNING;
|
||||
|
||||
while (result == BT::NodeStatus::RUNNING) {
|
||||
result = bt_handler->tree.tickRoot();
|
||||
|
||||
// Update goal on blackboard after Spin has been triggered once
|
||||
// to simulate a goal update during a recovery action
|
||||
if (server_handler->spin_server->getGoalCount() > 0) {
|
||||
geometry_msgs::msg::PoseStamped goal;
|
||||
goal.pose.position.x = 1.0;
|
||||
goal.pose.position.y = 1.0;
|
||||
goal.pose.position.z = 1.0;
|
||||
goal.pose.orientation.x = 0.0;
|
||||
goal.pose.orientation.y = 0.0;
|
||||
goal.pose.orientation.z = 0.0;
|
||||
goal.pose.orientation.w = 1.0;
|
||||
bt_handler->blackboard->set<geometry_msgs::msg::PoseStamped>("goal", goal); // NOLINT
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(10ms);
|
||||
}
|
||||
|
||||
// The final result should be success
|
||||
EXPECT_EQ(result, BT::NodeStatus::SUCCESS);
|
||||
|
||||
// ComputePathToPose is called 4 times
|
||||
EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 4);
|
||||
|
||||
// FollowPath is called 3 times
|
||||
EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 3);
|
||||
|
||||
// Local costmap is cleared 2 times
|
||||
EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 2);
|
||||
|
||||
// Global costmap is cleared 2 times
|
||||
EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 2);
|
||||
|
||||
// Spin server receives 1 action
|
||||
EXPECT_EQ(server_handler->spin_server->getGoalCount(), 1);
|
||||
|
||||
// All recovery action servers receive 0 goals
|
||||
EXPECT_EQ(server_handler->wait_server->getGoalCount(), 0);
|
||||
EXPECT_EQ(server_handler->backup_server->getGoalCount(), 0);
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,4 @@
|
||||
# Behavior Test
|
||||
|
||||
Provides some simple tests for behavior plugins.
|
||||
It creates an instance of the stack, with the behavior server loading different behavior plugins, and checks for successful behavior behaviors.
|
||||
@@ -0,0 +1,23 @@
|
||||
set(test_assisted_teleop_behavior test_assisted_teleop_behavior_node)
|
||||
|
||||
ament_add_gtest_executable(${test_assisted_teleop_behavior}
|
||||
test_assisted_teleop_behavior_node.cpp
|
||||
assisted_teleop_behavior_tester.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${test_assisted_teleop_behavior}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
ament_add_test(test_assisted_teleop_behavior
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_assisted_teleop_behavior_launch.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
TIMEOUT 180
|
||||
ENV
|
||||
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
|
||||
TEST_EXECUTABLE=$<TARGET_FILE:${test_assisted_teleop_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
|
||||
)
|
||||
+277
@@ -0,0 +1,277 @@
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <string>
|
||||
#include <random>
|
||||
#include <tuple>
|
||||
#include <memory>
|
||||
#include <iostream>
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
#include <iomanip>
|
||||
|
||||
#include "assisted_teleop_behavior_tester.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using namespace std::chrono; // NOLINT
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester()
|
||||
: is_active_(false),
|
||||
initial_pose_received_(false)
|
||||
{
|
||||
node_ = rclcpp::Node::make_shared("assisted_teleop_behavior_test");
|
||||
|
||||
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
|
||||
client_ptr_ = rclcpp_action::create_client<AssistedTeleop>(
|
||||
node_->get_node_base_interface(),
|
||||
node_->get_node_graph_interface(),
|
||||
node_->get_node_logging_interface(),
|
||||
node_->get_node_waitables_interface(),
|
||||
"assisted_teleop");
|
||||
|
||||
initial_pose_pub_ =
|
||||
node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
|
||||
|
||||
preempt_pub_ =
|
||||
node_->create_publisher<std_msgs::msg::Empty>("preempt_teleop", 10);
|
||||
|
||||
cmd_vel_pub_ =
|
||||
node_->create_publisher<geometry_msgs::msg::Twist>("cmd_vel_teleop", 10);
|
||||
|
||||
subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
|
||||
"amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
|
||||
std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
|
||||
|
||||
filtered_vel_sub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
|
||||
"cmd_vel",
|
||||
rclcpp::SystemDefaultsQoS(),
|
||||
std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1));
|
||||
|
||||
std::string costmap_topic = "/local_costmap/costmap_raw";
|
||||
std::string footprint_topic = "/local_costmap/published_footprint";
|
||||
|
||||
costmap_sub_ = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(
|
||||
node_,
|
||||
costmap_topic);
|
||||
|
||||
footprint_sub_ = std::make_shared<nav2_costmap_2d::FootprintSubscriber>(
|
||||
node_,
|
||||
footprint_topic,
|
||||
*tf_buffer_);
|
||||
|
||||
collision_checker_ = std::make_unique<nav2_costmap_2d::CostmapTopicCollisionChecker>(
|
||||
*costmap_sub_,
|
||||
*footprint_sub_
|
||||
);
|
||||
|
||||
stamp_ = node_->now();
|
||||
}
|
||||
|
||||
AssistedTeleopBehaviorTester::~AssistedTeleopBehaviorTester()
|
||||
{
|
||||
if (is_active_) {
|
||||
deactivate();
|
||||
}
|
||||
}
|
||||
|
||||
void AssistedTeleopBehaviorTester::activate()
|
||||
{
|
||||
if (is_active_) {
|
||||
throw std::runtime_error("Trying to activate while already active");
|
||||
return;
|
||||
}
|
||||
|
||||
while (!initial_pose_received_) {
|
||||
RCLCPP_WARN(node_->get_logger(), "Initial pose not received");
|
||||
sendInitialPose();
|
||||
std::this_thread::sleep_for(100ms);
|
||||
rclcpp::spin_some(node_);
|
||||
}
|
||||
|
||||
// Wait for lifecycle_manager_navigation to activate behavior_server
|
||||
std::this_thread::sleep_for(10s);
|
||||
|
||||
if (!client_ptr_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Action client not initialized");
|
||||
is_active_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!client_ptr_->wait_for_action_server(10s)) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting");
|
||||
is_active_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->node_->get_logger(), "Assisted Teleop action server is ready");
|
||||
is_active_ = true;
|
||||
}
|
||||
|
||||
void AssistedTeleopBehaviorTester::deactivate()
|
||||
{
|
||||
if (!is_active_) {
|
||||
throw std::runtime_error("Trying to deactivate while already inactive");
|
||||
}
|
||||
is_active_ = false;
|
||||
}
|
||||
|
||||
bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest(
|
||||
const float lin_vel,
|
||||
const float ang_vel)
|
||||
{
|
||||
if (!is_active_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Not activated");
|
||||
return false;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Sending goal");
|
||||
|
||||
auto goal_handle_future = client_ptr_->async_send_goal(nav2_msgs::action::AssistedTeleop::Goal());
|
||||
|
||||
if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp_action::ClientGoalHandle<AssistedTeleop>::SharedPtr goal_handle = goal_handle_future.get();
|
||||
if (!goal_handle) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Wait for the server to be done with the goal
|
||||
auto result_future = client_ptr_->async_get_result(goal_handle);
|
||||
|
||||
rclcpp::Rate r(1);
|
||||
|
||||
counter_ = 0;
|
||||
auto start_time = std::chrono::system_clock::now();
|
||||
while (rclcpp::ok()) {
|
||||
geometry_msgs::msg::Twist cmd_vel = geometry_msgs::msg::Twist();
|
||||
cmd_vel.linear.x = lin_vel;
|
||||
cmd_vel.angular.z = ang_vel;
|
||||
cmd_vel_pub_->publish(cmd_vel);
|
||||
|
||||
if (counter_ > 1) {
|
||||
break;
|
||||
}
|
||||
|
||||
auto current_time = std::chrono::system_clock::now();
|
||||
if (current_time - start_time > 25s) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Exceeded Timeout");
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp::spin_some(node_);
|
||||
r.sleep();
|
||||
}
|
||||
|
||||
auto preempt_msg = std_msgs::msg::Empty();
|
||||
preempt_pub_->publish(preempt_msg);
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for result");
|
||||
if (rclcpp::spin_until_future_complete(node_, result_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "get result call failed :(");
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp_action::ClientGoalHandle<AssistedTeleop>::WrappedResult
|
||||
wrapped_result = result_future.get();
|
||||
|
||||
switch (wrapped_result.code) {
|
||||
case rclcpp_action::ResultCode::SUCCEEDED: break;
|
||||
case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal was aborted");
|
||||
return false;
|
||||
case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal was canceled");
|
||||
return false;
|
||||
default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
|
||||
return false;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "result received");
|
||||
|
||||
geometry_msgs::msg::PoseStamped current_pose;
|
||||
if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
|
||||
return false;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Pose2D pose_2d;
|
||||
pose_2d.x = current_pose.pose.position.x;
|
||||
pose_2d.y = current_pose.pose.position.y;
|
||||
pose_2d.theta = tf2::getYaw(current_pose.pose.orientation);
|
||||
|
||||
if (!collision_checker_->isCollisionFree(pose_2d)) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Ended in collision");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void AssistedTeleopBehaviorTester::sendInitialPose()
|
||||
{
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped pose;
|
||||
pose.header.frame_id = "map";
|
||||
pose.header.stamp = stamp_;
|
||||
pose.pose.pose.position.x = -2.0;
|
||||
pose.pose.pose.position.y = -0.5;
|
||||
pose.pose.pose.position.z = 0.0;
|
||||
pose.pose.pose.orientation.x = 0.0;
|
||||
pose.pose.pose.orientation.y = 0.0;
|
||||
pose.pose.pose.orientation.z = 0.0;
|
||||
pose.pose.pose.orientation.w = 1.0;
|
||||
for (int i = 0; i < 35; i++) {
|
||||
pose.pose.covariance[i] = 0.0;
|
||||
}
|
||||
pose.pose.covariance[0] = 0.08;
|
||||
pose.pose.covariance[7] = 0.08;
|
||||
pose.pose.covariance[35] = 0.05;
|
||||
|
||||
initial_pose_pub_->publish(pose);
|
||||
RCLCPP_INFO(node_->get_logger(), "Sent initial pose");
|
||||
}
|
||||
|
||||
void AssistedTeleopBehaviorTester::amclPoseCallback(
|
||||
const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr)
|
||||
{
|
||||
initial_pose_received_ = true;
|
||||
}
|
||||
|
||||
void AssistedTeleopBehaviorTester::filteredVelCallback(
|
||||
geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
if (msg->linear.x == 0.0f) {
|
||||
counter_++;
|
||||
} else {
|
||||
counter_ = 0;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
+104
@@ -0,0 +1,104 @@
|
||||
// Copyright (c) 2020 Samsung Research
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_
|
||||
#define BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <algorithm>
|
||||
|
||||
#include "angles/angles.h"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "geometry_msgs/msg/pose2_d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
|
||||
#include "nav2_msgs/action/assisted_teleop.hpp"
|
||||
#include "nav2_util/node_thread.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "std_msgs/msg/empty.hpp"
|
||||
|
||||
#include "tf2/utils.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
class AssistedTeleopBehaviorTester
|
||||
{
|
||||
public:
|
||||
using AssistedTeleop = nav2_msgs::action::AssistedTeleop;
|
||||
|
||||
AssistedTeleopBehaviorTester();
|
||||
~AssistedTeleopBehaviorTester();
|
||||
|
||||
// Runs a single test with given target yaw
|
||||
bool defaultAssistedTeleopTest(
|
||||
const float lin_vel,
|
||||
const float ang_vel);
|
||||
|
||||
void activate();
|
||||
|
||||
void deactivate();
|
||||
|
||||
bool isActive() const
|
||||
{
|
||||
return is_active_;
|
||||
}
|
||||
|
||||
private:
|
||||
void sendInitialPose();
|
||||
|
||||
void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
|
||||
|
||||
void filteredVelCallback(geometry_msgs::msg::Twist::SharedPtr msg);
|
||||
|
||||
unsigned int counter_;
|
||||
bool is_active_;
|
||||
bool initial_pose_received_;
|
||||
rclcpp::Time stamp_;
|
||||
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
// Publishers
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
|
||||
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr preempt_pub_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
|
||||
|
||||
// Subscribers
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr filtered_vel_sub_;
|
||||
|
||||
// Action client to call AssistedTeleop action
|
||||
rclcpp_action::Client<AssistedTeleop>::SharedPtr client_ptr_;
|
||||
|
||||
// collision checking
|
||||
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
|
||||
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
|
||||
std::unique_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
|
||||
};
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
|
||||
#endif // BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_
|
||||
Executable
+103
@@ -0,0 +1,103 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright (c) 2012 Samsung Research America
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import os
|
||||
import sys
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch import LaunchService
|
||||
from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
map_yaml_file = os.getenv('TEST_MAP')
|
||||
world = os.getenv('TEST_WORLD')
|
||||
|
||||
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')
|
||||
|
||||
# Replace the `use_astar` setting on the params file
|
||||
configured_params = RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key='',
|
||||
param_rewrites='',
|
||||
convert_types=True)
|
||||
|
||||
return LaunchDescription([
|
||||
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'),
|
||||
|
||||
# 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']),
|
||||
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={
|
||||
'map': map_yaml_file,
|
||||
'use_sim_time': 'True',
|
||||
'params_file': configured_params,
|
||||
'bt_xml_file': bt_navigator_xml,
|
||||
'autostart': 'True'}.items()),
|
||||
])
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
ld = generate_launch_description()
|
||||
|
||||
testExecutable = os.getenv('TEST_EXECUTABLE')
|
||||
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[testExecutable],
|
||||
name='test_assisted_teleop_behavior_node',
|
||||
output='screen')
|
||||
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
+114
@@ -0,0 +1,114 @@
|
||||
// Copyright (c) 2020 Samsung Research
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2022 Joshua Wallace
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <cmath>
|
||||
#include <tuple>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "assisted_teleop_behavior_tester.hpp"
|
||||
#include "nav2_msgs/action/back_up.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
using nav2_system_tests::AssistedTeleopBehaviorTester;
|
||||
|
||||
struct TestParameters
|
||||
{
|
||||
float lin_vel;
|
||||
float ang_vel;
|
||||
};
|
||||
|
||||
|
||||
std::string testNameGenerator(const testing::TestParamInfo<TestParameters> &)
|
||||
{
|
||||
static int test_index = 0;
|
||||
std::string name = "AssistedTeleopTest" + std::to_string(test_index);
|
||||
++test_index;
|
||||
return name;
|
||||
}
|
||||
|
||||
class AssistedTeleopBehaviorTestFixture
|
||||
: public ::testing::TestWithParam<TestParameters>
|
||||
{
|
||||
public:
|
||||
static void SetUpTestCase()
|
||||
{
|
||||
assisted_teleop_behavior_tester = new AssistedTeleopBehaviorTester();
|
||||
if (!assisted_teleop_behavior_tester->isActive()) {
|
||||
assisted_teleop_behavior_tester->activate();
|
||||
}
|
||||
}
|
||||
|
||||
static void TearDownTestCase()
|
||||
{
|
||||
delete assisted_teleop_behavior_tester;
|
||||
assisted_teleop_behavior_tester = nullptr;
|
||||
}
|
||||
|
||||
protected:
|
||||
static AssistedTeleopBehaviorTester * assisted_teleop_behavior_tester;
|
||||
};
|
||||
|
||||
AssistedTeleopBehaviorTester *
|
||||
AssistedTeleopBehaviorTestFixture::assisted_teleop_behavior_tester = nullptr;
|
||||
|
||||
TEST_P(AssistedTeleopBehaviorTestFixture, testAssistedTeleopBehavior)
|
||||
{
|
||||
auto test_params = GetParam();
|
||||
|
||||
if (!assisted_teleop_behavior_tester->isActive()) {
|
||||
assisted_teleop_behavior_tester->activate();
|
||||
}
|
||||
|
||||
bool success = false;
|
||||
success = assisted_teleop_behavior_tester->defaultAssistedTeleopTest(
|
||||
test_params.lin_vel,
|
||||
test_params.ang_vel);
|
||||
|
||||
EXPECT_TRUE(success);
|
||||
}
|
||||
|
||||
std::vector<TestParameters> test_params = {TestParameters{-0.1, 0.0},
|
||||
TestParameters{0.35, 0.05}};
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
TestAssistedTeleopBehavior,
|
||||
AssistedTeleopBehaviorTestFixture,
|
||||
::testing::Values(
|
||||
test_params[0],
|
||||
test_params[1]),
|
||||
testNameGenerator
|
||||
);
|
||||
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
set(test_backup_behavior test_backup_behavior_node)
|
||||
|
||||
ament_add_gtest_executable(${test_backup_behavior}
|
||||
test_backup_behavior_node.cpp
|
||||
backup_behavior_tester.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${test_backup_behavior}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
ament_add_test(test_backup_recovery
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_backup_behavior_launch.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
TIMEOUT 180
|
||||
ENV
|
||||
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
|
||||
TEST_EXECUTABLE=$<TARGET_FILE:${test_backup_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
|
||||
)
|
||||
@@ -0,0 +1,223 @@
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <string>
|
||||
#include <random>
|
||||
#include <tuple>
|
||||
#include <memory>
|
||||
#include <iostream>
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
#include <iomanip>
|
||||
|
||||
#include "backup_behavior_tester.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using namespace std::chrono; // NOLINT
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
BackupBehaviorTester::BackupBehaviorTester()
|
||||
: is_active_(false),
|
||||
initial_pose_received_(false)
|
||||
{
|
||||
rclcpp::NodeOptions options;
|
||||
options.parameter_overrides({{"use_sim_time", true}});
|
||||
node_ = rclcpp::Node::make_shared("backup_behavior_test", options);
|
||||
|
||||
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
|
||||
client_ptr_ = rclcpp_action::create_client<BackUp>(
|
||||
node_->get_node_base_interface(),
|
||||
node_->get_node_graph_interface(),
|
||||
node_->get_node_logging_interface(),
|
||||
node_->get_node_waitables_interface(),
|
||||
"backup");
|
||||
|
||||
publisher_ =
|
||||
node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
|
||||
|
||||
subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
|
||||
"amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
|
||||
std::bind(&BackupBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
|
||||
|
||||
stamp_ = node_->now();
|
||||
}
|
||||
|
||||
BackupBehaviorTester::~BackupBehaviorTester()
|
||||
{
|
||||
if (is_active_) {
|
||||
deactivate();
|
||||
}
|
||||
}
|
||||
|
||||
void BackupBehaviorTester::activate()
|
||||
{
|
||||
if (is_active_) {
|
||||
throw std::runtime_error("Trying to activate while already active");
|
||||
return;
|
||||
}
|
||||
|
||||
while (!initial_pose_received_) {
|
||||
RCLCPP_WARN(node_->get_logger(), "Initial pose not received");
|
||||
sendInitialPose();
|
||||
std::this_thread::sleep_for(100ms);
|
||||
rclcpp::spin_some(node_);
|
||||
}
|
||||
|
||||
// Wait for lifecycle_manager_navigation to activate behavior_server
|
||||
std::this_thread::sleep_for(10s);
|
||||
|
||||
if (!client_ptr_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Action client not initialized");
|
||||
is_active_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!client_ptr_->wait_for_action_server(10s)) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting");
|
||||
is_active_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->node_->get_logger(), "Backup action server is ready");
|
||||
is_active_ = true;
|
||||
}
|
||||
|
||||
void BackupBehaviorTester::deactivate()
|
||||
{
|
||||
if (!is_active_) {
|
||||
throw std::runtime_error("Trying to deactivate while already inactive");
|
||||
}
|
||||
is_active_ = false;
|
||||
}
|
||||
|
||||
bool BackupBehaviorTester::defaultBackupBehaviorTest(
|
||||
const BackUp::Goal goal_msg,
|
||||
const double tolerance)
|
||||
{
|
||||
if (!is_active_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Not activated");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Sleep to let behavior server be ready for serving in multiple runs
|
||||
std::this_thread::sleep_for(5s);
|
||||
|
||||
RCLCPP_INFO(this->node_->get_logger(), "Sending goal");
|
||||
|
||||
geometry_msgs::msg::PoseStamped initial_pose;
|
||||
if (!nav2_util::getCurrentPose(initial_pose, *tf_buffer_, "odom")) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
|
||||
return false;
|
||||
}
|
||||
RCLCPP_INFO(node_->get_logger(), "Found current robot pose");
|
||||
|
||||
auto goal_handle_future = client_ptr_->async_send_goal(goal_msg);
|
||||
|
||||
if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp_action::ClientGoalHandle<BackUp>::SharedPtr goal_handle = goal_handle_future.get();
|
||||
if (!goal_handle) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Wait for the server to be done with the goal
|
||||
auto result_future = client_ptr_->async_get_result(goal_handle);
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for result");
|
||||
if (rclcpp::spin_until_future_complete(node_, result_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "get result call failed :(");
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp_action::ClientGoalHandle<BackUp>::WrappedResult wrapped_result = result_future.get();
|
||||
|
||||
switch (wrapped_result.code) {
|
||||
case rclcpp_action::ResultCode::SUCCEEDED: break;
|
||||
case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal was aborted");
|
||||
return false;
|
||||
case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal was canceled");
|
||||
return false;
|
||||
default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
|
||||
return false;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "result received");
|
||||
|
||||
geometry_msgs::msg::PoseStamped current_pose;
|
||||
if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
|
||||
return false;
|
||||
}
|
||||
|
||||
double dist = nav2_util::geometry_utils::euclidean_distance(initial_pose, current_pose);
|
||||
|
||||
if (fabs(dist) > fabs(goal_msg.target.x) + tolerance) {
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Distance from goal is %lf (tolerance %lf)",
|
||||
fabs(dist - goal_msg.target.x), tolerance);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void BackupBehaviorTester::sendInitialPose()
|
||||
{
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped pose;
|
||||
pose.header.frame_id = "map";
|
||||
pose.header.stamp = stamp_;
|
||||
pose.pose.pose.position.x = -2.0;
|
||||
pose.pose.pose.position.y = -0.5;
|
||||
pose.pose.pose.position.z = 0.0;
|
||||
pose.pose.pose.orientation.x = 0.0;
|
||||
pose.pose.pose.orientation.y = 0.0;
|
||||
pose.pose.pose.orientation.z = 0.0;
|
||||
pose.pose.pose.orientation.w = 1.0;
|
||||
for (int i = 0; i < 35; i++) {
|
||||
pose.pose.covariance[i] = 0.0;
|
||||
}
|
||||
pose.pose.covariance[0] = 0.08;
|
||||
pose.pose.covariance[7] = 0.08;
|
||||
pose.pose.covariance[35] = 0.05;
|
||||
|
||||
publisher_->publish(pose);
|
||||
RCLCPP_INFO(node_->get_logger(), "Sent initial pose");
|
||||
}
|
||||
|
||||
void BackupBehaviorTester::amclPoseCallback(
|
||||
const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr)
|
||||
{
|
||||
initial_pose_received_ = true;
|
||||
}
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
@@ -0,0 +1,90 @@
|
||||
// Copyright (c) 2020 Samsung Research
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_
|
||||
#define BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <algorithm>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "angles/angles.h"
|
||||
#include "nav2_msgs/action/back_up.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "nav2_util/node_thread.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
|
||||
|
||||
#include "tf2/utils.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
class BackupBehaviorTester
|
||||
{
|
||||
public:
|
||||
using BackUp = nav2_msgs::action::BackUp;
|
||||
using GoalHandleBackup = rclcpp_action::ClientGoalHandle<BackUp>;
|
||||
|
||||
BackupBehaviorTester();
|
||||
~BackupBehaviorTester();
|
||||
|
||||
// Runs a single test with given target yaw
|
||||
bool defaultBackupBehaviorTest(
|
||||
const BackUp::Goal goal_msg,
|
||||
const double tolerance);
|
||||
|
||||
void activate();
|
||||
|
||||
void deactivate();
|
||||
|
||||
bool isActive() const
|
||||
{
|
||||
return is_active_;
|
||||
}
|
||||
|
||||
private:
|
||||
void sendInitialPose();
|
||||
|
||||
void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
|
||||
|
||||
bool is_active_;
|
||||
bool initial_pose_received_;
|
||||
rclcpp::Time stamp_;
|
||||
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
// Publisher to publish initial pose
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
|
||||
|
||||
// Subscriber for amcl pose
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
|
||||
|
||||
// Action client to call Backup action
|
||||
rclcpp_action::Client<BackUp>::SharedPtr client_ptr_;
|
||||
};
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
|
||||
#endif // BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_
|
||||
+102
@@ -0,0 +1,102 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright (c) 2012 Samsung Research America
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import os
|
||||
import sys
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch import LaunchService
|
||||
from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
map_yaml_file = os.getenv('TEST_MAP')
|
||||
world = os.getenv('TEST_WORLD')
|
||||
|
||||
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')
|
||||
|
||||
# Replace the `use_astar` setting on the params file
|
||||
configured_params = RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key='',
|
||||
param_rewrites='',
|
||||
convert_types=True)
|
||||
|
||||
return LaunchDescription([
|
||||
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'),
|
||||
|
||||
# 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']),
|
||||
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={
|
||||
'map': map_yaml_file,
|
||||
'use_sim_time': 'True',
|
||||
'params_file': configured_params,
|
||||
'bt_xml_file': bt_navigator_xml,
|
||||
'autostart': 'True'}.items()),
|
||||
])
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
ld = generate_launch_description()
|
||||
|
||||
testExecutable = os.getenv('TEST_EXECUTABLE')
|
||||
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[testExecutable], name='test_backup_behavior_node', output='screen',
|
||||
)
|
||||
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,128 @@
|
||||
// Copyright (c) 2020 Samsung Research
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2022 Joshua Wallace
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <cmath>
|
||||
#include <tuple>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "backup_behavior_tester.hpp"
|
||||
#include "nav2_msgs/action/back_up.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
using nav2_system_tests::BackupBehaviorTester;
|
||||
|
||||
struct TestParameters
|
||||
{
|
||||
float x;
|
||||
float y;
|
||||
float speed;
|
||||
float tolerance;
|
||||
};
|
||||
|
||||
|
||||
std::string testNameGenerator(const testing::TestParamInfo<TestParameters> &)
|
||||
{
|
||||
static int test_index = 0;
|
||||
std::string name = "BackUpTest" + std::to_string(test_index);
|
||||
++test_index;
|
||||
return name;
|
||||
}
|
||||
|
||||
class BackupBehaviorTestFixture
|
||||
: public ::testing::TestWithParam<TestParameters>
|
||||
{
|
||||
public:
|
||||
static void SetUpTestCase()
|
||||
{
|
||||
backup_behavior_tester = new BackupBehaviorTester();
|
||||
if (!backup_behavior_tester->isActive()) {
|
||||
backup_behavior_tester->activate();
|
||||
}
|
||||
}
|
||||
|
||||
static void TearDownTestCase()
|
||||
{
|
||||
delete backup_behavior_tester;
|
||||
backup_behavior_tester = nullptr;
|
||||
}
|
||||
|
||||
protected:
|
||||
static BackupBehaviorTester * backup_behavior_tester;
|
||||
};
|
||||
|
||||
BackupBehaviorTester * BackupBehaviorTestFixture::backup_behavior_tester = nullptr;
|
||||
|
||||
TEST_P(BackupBehaviorTestFixture, testBackupBehavior)
|
||||
{
|
||||
auto test_params = GetParam();
|
||||
auto goal = nav2_msgs::action::BackUp::Goal();
|
||||
goal.target.x = test_params.x;
|
||||
goal.target.y = test_params.y;
|
||||
goal.speed = test_params.speed;
|
||||
float tolerance = test_params.tolerance;
|
||||
|
||||
if (!backup_behavior_tester->isActive()) {
|
||||
backup_behavior_tester->activate();
|
||||
}
|
||||
|
||||
bool success = false;
|
||||
success = backup_behavior_tester->defaultBackupBehaviorTest(goal, tolerance);
|
||||
|
||||
float dist_to_obstacle = 2.0f;
|
||||
|
||||
if ( ((dist_to_obstacle - std::fabs(test_params.x)) < std::fabs(goal.speed)) ||
|
||||
std::fabs(goal.target.y) > 0)
|
||||
{
|
||||
EXPECT_FALSE(success);
|
||||
} else {
|
||||
EXPECT_TRUE(success);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<TestParameters> test_params = {TestParameters{-0.05, 0.0, -0.2, 0.01},
|
||||
TestParameters{-0.05, 0.1, -0.2, 0.01},
|
||||
TestParameters{-2.0, 0.0, -0.2, 0.1}};
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
BackupBehaviorTests,
|
||||
BackupBehaviorTestFixture,
|
||||
::testing::Values(
|
||||
test_params[0],
|
||||
test_params[1],
|
||||
test_params[2]),
|
||||
testNameGenerator
|
||||
);
|
||||
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
set(test_drive_on_heading_behavior test_drive_on_heading_behavior_node)
|
||||
|
||||
ament_add_gtest_executable(${test_drive_on_heading_behavior}
|
||||
test_drive_on_heading_behavior_node.cpp
|
||||
drive_on_heading_behavior_tester.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${test_drive_on_heading_behavior}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
ament_add_test(test_drive_on_heading_recovery
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_drive_on_heading_behavior_launch.py"
|
||||
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
|
||||
)
|
||||
+224
@@ -0,0 +1,224 @@
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <string>
|
||||
#include <random>
|
||||
#include <tuple>
|
||||
#include <memory>
|
||||
#include <iostream>
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
#include <iomanip>
|
||||
|
||||
#include "drive_on_heading_behavior_tester.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using namespace std::chrono; // NOLINT
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
DriveOnHeadingBehaviorTester::DriveOnHeadingBehaviorTester()
|
||||
: is_active_(false),
|
||||
initial_pose_received_(false)
|
||||
{
|
||||
rclcpp::NodeOptions options;
|
||||
options.parameter_overrides({{"use_sim_time", true}});
|
||||
node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test", options);
|
||||
|
||||
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
|
||||
client_ptr_ = rclcpp_action::create_client<DriveOnHeading>(
|
||||
node_->get_node_base_interface(),
|
||||
node_->get_node_graph_interface(),
|
||||
node_->get_node_logging_interface(),
|
||||
node_->get_node_waitables_interface(),
|
||||
"drive_on_heading");
|
||||
|
||||
publisher_ =
|
||||
node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
|
||||
|
||||
subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
|
||||
"amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
|
||||
std::bind(&DriveOnHeadingBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
|
||||
|
||||
stamp_ = node_->now();
|
||||
}
|
||||
|
||||
DriveOnHeadingBehaviorTester::~DriveOnHeadingBehaviorTester()
|
||||
{
|
||||
if (is_active_) {
|
||||
deactivate();
|
||||
}
|
||||
}
|
||||
|
||||
void DriveOnHeadingBehaviorTester::activate()
|
||||
{
|
||||
if (is_active_) {
|
||||
throw std::runtime_error("Trying to activate while already active");
|
||||
return;
|
||||
}
|
||||
|
||||
while (!initial_pose_received_) {
|
||||
RCLCPP_WARN(node_->get_logger(), "Initial pose not received");
|
||||
sendInitialPose();
|
||||
std::this_thread::sleep_for(100ms);
|
||||
rclcpp::spin_some(node_);
|
||||
}
|
||||
|
||||
// Wait for lifecycle_manager_navigation to activate behavior_server
|
||||
std::this_thread::sleep_for(10s);
|
||||
|
||||
if (!client_ptr_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Action client not initialized");
|
||||
is_active_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!client_ptr_->wait_for_action_server(10s)) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting");
|
||||
is_active_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->node_->get_logger(), "DriveOnHeading action server is ready");
|
||||
is_active_ = true;
|
||||
}
|
||||
|
||||
void DriveOnHeadingBehaviorTester::deactivate()
|
||||
{
|
||||
if (!is_active_) {
|
||||
throw std::runtime_error("Trying to deactivate while already inactive");
|
||||
}
|
||||
is_active_ = false;
|
||||
}
|
||||
|
||||
bool DriveOnHeadingBehaviorTester::defaultDriveOnHeadingBehaviorTest(
|
||||
const DriveOnHeading::Goal goal_msg,
|
||||
const double tolerance)
|
||||
{
|
||||
if (!is_active_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Not activated");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Sleep to let behavior server be ready for serving in multiple runs
|
||||
std::this_thread::sleep_for(5s);
|
||||
|
||||
RCLCPP_INFO(this->node_->get_logger(), "Sending goal");
|
||||
|
||||
geometry_msgs::msg::PoseStamped initial_pose;
|
||||
if (!nav2_util::getCurrentPose(initial_pose, *tf_buffer_, "odom")) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
|
||||
return false;
|
||||
}
|
||||
RCLCPP_INFO(node_->get_logger(), "Found current robot pose");
|
||||
|
||||
auto goal_handle_future = client_ptr_->async_send_goal(goal_msg);
|
||||
|
||||
if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp_action::ClientGoalHandle<DriveOnHeading>::SharedPtr goal_handle = goal_handle_future.get();
|
||||
if (!goal_handle) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Wait for the server to be done with the goal
|
||||
auto result_future = client_ptr_->async_get_result(goal_handle);
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for result");
|
||||
if (rclcpp::spin_until_future_complete(node_, result_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "get result call failed :(");
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp_action::ClientGoalHandle<DriveOnHeading>::WrappedResult wrapped_result =
|
||||
result_future.get();
|
||||
|
||||
switch (wrapped_result.code) {
|
||||
case rclcpp_action::ResultCode::SUCCEEDED: break;
|
||||
case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal was aborted");
|
||||
return false;
|
||||
case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal was canceled");
|
||||
return false;
|
||||
default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
|
||||
return false;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "result received");
|
||||
|
||||
geometry_msgs::msg::PoseStamped current_pose;
|
||||
if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
|
||||
return false;
|
||||
}
|
||||
|
||||
double dist = nav2_util::geometry_utils::euclidean_distance(initial_pose, current_pose);
|
||||
|
||||
if (fabs(dist) > fabs(goal_msg.target.x) + tolerance) {
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Distance from goal is %lf (tolerance %lf)",
|
||||
fabs(dist - goal_msg.target.x), tolerance);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void DriveOnHeadingBehaviorTester::sendInitialPose()
|
||||
{
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped pose;
|
||||
pose.header.frame_id = "map";
|
||||
pose.header.stamp = stamp_;
|
||||
pose.pose.pose.position.x = -2.0;
|
||||
pose.pose.pose.position.y = -0.5;
|
||||
pose.pose.pose.position.z = 0.0;
|
||||
pose.pose.pose.orientation.x = 0.0;
|
||||
pose.pose.pose.orientation.y = 0.0;
|
||||
pose.pose.pose.orientation.z = 0.0;
|
||||
pose.pose.pose.orientation.w = 1.0;
|
||||
for (int i = 0; i < 35; i++) {
|
||||
pose.pose.covariance[i] = 0.0;
|
||||
}
|
||||
pose.pose.covariance[0] = 0.08;
|
||||
pose.pose.covariance[7] = 0.08;
|
||||
pose.pose.covariance[35] = 0.05;
|
||||
|
||||
publisher_->publish(pose);
|
||||
RCLCPP_INFO(node_->get_logger(), "Sent initial pose");
|
||||
}
|
||||
|
||||
void DriveOnHeadingBehaviorTester::amclPoseCallback(
|
||||
const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr)
|
||||
{
|
||||
initial_pose_received_ = true;
|
||||
}
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
+90
@@ -0,0 +1,90 @@
|
||||
// Copyright (c) 2020 Samsung Research
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_
|
||||
#define BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <algorithm>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "angles/angles.h"
|
||||
#include "nav2_msgs/action/drive_on_heading.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "nav2_util/node_thread.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
|
||||
|
||||
#include "tf2/utils.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
class DriveOnHeadingBehaviorTester
|
||||
{
|
||||
public:
|
||||
using DriveOnHeading = nav2_msgs::action::DriveOnHeading;
|
||||
using GoalHandleDriveOnHeading = rclcpp_action::ClientGoalHandle<DriveOnHeading>;
|
||||
|
||||
DriveOnHeadingBehaviorTester();
|
||||
~DriveOnHeadingBehaviorTester();
|
||||
|
||||
// Runs a single test with given target yaw
|
||||
bool defaultDriveOnHeadingBehaviorTest(
|
||||
const DriveOnHeading::Goal goal_msg,
|
||||
double tolerance);
|
||||
|
||||
void activate();
|
||||
|
||||
void deactivate();
|
||||
|
||||
bool isActive() const
|
||||
{
|
||||
return is_active_;
|
||||
}
|
||||
|
||||
private:
|
||||
void sendInitialPose();
|
||||
|
||||
void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
|
||||
|
||||
bool is_active_;
|
||||
bool initial_pose_received_;
|
||||
rclcpp::Time stamp_;
|
||||
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
// Publisher to publish initial pose
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
|
||||
|
||||
// Subscriber for amcl pose
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
|
||||
|
||||
// Action client to call DriveOnHeading action
|
||||
rclcpp_action::Client<DriveOnHeading>::SharedPtr client_ptr_;
|
||||
};
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
|
||||
#endif // BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_
|
||||
+103
@@ -0,0 +1,103 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright (c) 2012 Samsung Research America
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import os
|
||||
import sys
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch import LaunchService
|
||||
from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
map_yaml_file = os.getenv('TEST_MAP')
|
||||
world = os.getenv('TEST_WORLD')
|
||||
|
||||
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')
|
||||
|
||||
# Replace the `use_astar` setting on the params file
|
||||
configured_params = RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key='',
|
||||
param_rewrites='',
|
||||
convert_types=True)
|
||||
|
||||
return LaunchDescription([
|
||||
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'),
|
||||
|
||||
# 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']),
|
||||
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={
|
||||
'map': map_yaml_file,
|
||||
'use_sim_time': 'True',
|
||||
'params_file': configured_params,
|
||||
'bt_xml_file': bt_navigator_xml,
|
||||
'autostart': 'True'}.items()),
|
||||
])
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
ld = generate_launch_description()
|
||||
|
||||
testExecutable = os.getenv('TEST_EXECUTABLE')
|
||||
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[testExecutable],
|
||||
name='test_drive_on_heading_behavior_node',
|
||||
output='screen')
|
||||
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
+133
@@ -0,0 +1,133 @@
|
||||
// Copyright (c) 2020 Samsung Research
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <cmath>
|
||||
#include <tuple>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "drive_on_heading_behavior_tester.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
using nav2_system_tests::DriveOnHeadingBehaviorTester;
|
||||
|
||||
struct TestParameters
|
||||
{
|
||||
float x;
|
||||
float y;
|
||||
float speed;
|
||||
float time_allowance;
|
||||
float tolerance;
|
||||
};
|
||||
|
||||
std::string testNameGenerator(const testing::TestParamInfo<TestParameters> &)
|
||||
{
|
||||
static int test_index = 0;
|
||||
std::string name = "DriveOnHeadingTest" + std::to_string(test_index);
|
||||
++test_index;
|
||||
return name;
|
||||
}
|
||||
|
||||
class DriveOnHeadingBehaviorTestFixture
|
||||
: public ::testing::TestWithParam<TestParameters>
|
||||
{
|
||||
public:
|
||||
static void SetUpTestCase()
|
||||
{
|
||||
drive_on_heading_behavior_tester = new DriveOnHeadingBehaviorTester();
|
||||
if (!drive_on_heading_behavior_tester->isActive()) {
|
||||
drive_on_heading_behavior_tester->activate();
|
||||
}
|
||||
}
|
||||
|
||||
static void TearDownTestCase()
|
||||
{
|
||||
delete drive_on_heading_behavior_tester;
|
||||
drive_on_heading_behavior_tester = nullptr;
|
||||
}
|
||||
|
||||
protected:
|
||||
static DriveOnHeadingBehaviorTester * drive_on_heading_behavior_tester;
|
||||
};
|
||||
|
||||
DriveOnHeadingBehaviorTester * DriveOnHeadingBehaviorTestFixture::drive_on_heading_behavior_tester =
|
||||
nullptr;
|
||||
|
||||
TEST_P(DriveOnHeadingBehaviorTestFixture, testBackupBehavior)
|
||||
{
|
||||
auto test_params = GetParam();
|
||||
auto goal = nav2_msgs::action::DriveOnHeading::Goal();
|
||||
goal.target.x = test_params.x;
|
||||
goal.target.y = test_params.y;
|
||||
goal.speed = test_params.speed;
|
||||
goal.time_allowance.sec = test_params.time_allowance;
|
||||
float tolerance = test_params.tolerance;
|
||||
|
||||
if (!drive_on_heading_behavior_tester->isActive()) {
|
||||
drive_on_heading_behavior_tester->activate();
|
||||
}
|
||||
|
||||
bool success = false;
|
||||
success = drive_on_heading_behavior_tester->defaultDriveOnHeadingBehaviorTest(
|
||||
goal,
|
||||
tolerance);
|
||||
|
||||
float dist_to_obstacle = 2.0f;
|
||||
if ( ((dist_to_obstacle - std::fabs(test_params.x)) < std::fabs(goal.speed)) ||
|
||||
std::fabs(goal.target.y) > 0 ||
|
||||
goal.time_allowance.sec < 2.0 ||
|
||||
!((goal.target.x > 0.0) == (goal.speed > 0.0)))
|
||||
{
|
||||
EXPECT_FALSE(success);
|
||||
} else {
|
||||
EXPECT_TRUE(success);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<TestParameters> test_params = {TestParameters{-0.05, 0.0, -0.2, 10.0, 0.01},
|
||||
TestParameters{-0.05, 0.1, -0.2, 10.0, 0.01},
|
||||
TestParameters{-2.0, 0.0, -0.2, 10.0, 0.1},
|
||||
TestParameters{-0.05, 0.0, -0.01, 1.0, 0.01},
|
||||
TestParameters{0.05, 0.0, -0.2, 10.0, 0.01}};
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
DriveOnHeadingBehaviorTests,
|
||||
DriveOnHeadingBehaviorTestFixture,
|
||||
::testing::Values(
|
||||
test_params[0],
|
||||
test_params[1],
|
||||
test_params[2],
|
||||
test_params[3],
|
||||
test_params[4]),
|
||||
testNameGenerator);
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
set(test_spin_behavior_exec test_spin_behavior_node)
|
||||
|
||||
ament_add_gtest_executable(${test_spin_behavior_exec}
|
||||
test_spin_behavior_node.cpp
|
||||
spin_behavior_tester.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${test_spin_behavior_exec}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
ament_add_test(test_spin_behavior
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_behavior_launch.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
TIMEOUT 180
|
||||
ENV
|
||||
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
|
||||
TEST_EXECUTABLE=$<TARGET_FILE:${test_spin_behavior_exec}>
|
||||
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
|
||||
)
|
||||
|
||||
ament_add_test(test_spin_behavior_fake
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_behavior_fake_launch.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
TIMEOUT 180
|
||||
ENV
|
||||
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
|
||||
TEST_EXECUTABLE=$<TARGET_FILE:${test_spin_behavior_exec}>
|
||||
MAKE_FAKE_COSTMAP=true
|
||||
)
|
||||
@@ -0,0 +1,351 @@
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <string>
|
||||
#include <random>
|
||||
#include <tuple>
|
||||
#include <memory>
|
||||
#include <iostream>
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
#include <iomanip>
|
||||
|
||||
#include "spin_behavior_tester.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using namespace std::chrono; // NOLINT
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
SpinBehaviorTester::SpinBehaviorTester()
|
||||
: is_active_(false),
|
||||
initial_pose_received_(false)
|
||||
{
|
||||
node_ = rclcpp::Node::make_shared("spin_behavior_test");
|
||||
|
||||
|
||||
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
|
||||
if (std::getenv("MAKE_FAKE_COSTMAP") != NULL) {
|
||||
// if this variable is set, make a fake costmap
|
||||
make_fake_costmap_ = true;
|
||||
} else {
|
||||
make_fake_costmap_ = false;
|
||||
}
|
||||
|
||||
client_ptr_ = rclcpp_action::create_client<Spin>(
|
||||
node_->get_node_base_interface(),
|
||||
node_->get_node_graph_interface(),
|
||||
node_->get_node_logging_interface(),
|
||||
node_->get_node_waitables_interface(),
|
||||
"spin");
|
||||
|
||||
publisher_ =
|
||||
node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
|
||||
fake_costmap_publisher_ =
|
||||
node_->create_publisher<nav2_msgs::msg::Costmap>(
|
||||
"local_costmap/costmap_raw",
|
||||
rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
|
||||
fake_footprint_publisher_ = node_->create_publisher<geometry_msgs::msg::PolygonStamped>(
|
||||
"local_costmap/published_footprint", rclcpp::SystemDefaultsQoS());
|
||||
|
||||
subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
|
||||
"amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
|
||||
std::bind(&SpinBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
|
||||
|
||||
stamp_ = node_->now();
|
||||
}
|
||||
|
||||
SpinBehaviorTester::~SpinBehaviorTester()
|
||||
{
|
||||
if (is_active_) {
|
||||
deactivate();
|
||||
}
|
||||
}
|
||||
|
||||
void SpinBehaviorTester::activate()
|
||||
{
|
||||
if (is_active_) {
|
||||
throw std::runtime_error("Trying to activate while already active");
|
||||
return;
|
||||
}
|
||||
if (!make_fake_costmap_) {
|
||||
while (!initial_pose_received_) {
|
||||
RCLCPP_WARN(node_->get_logger(), "Initial pose not received");
|
||||
sendInitialPose();
|
||||
std::this_thread::sleep_for(100ms);
|
||||
rclcpp::spin_some(node_);
|
||||
}
|
||||
} else {
|
||||
sendFakeOdom(0.0);
|
||||
}
|
||||
|
||||
// Wait for lifecycle_manager_navigation to activate behavior_server
|
||||
std::this_thread::sleep_for(10s);
|
||||
|
||||
if (!client_ptr_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Action client not initialized");
|
||||
is_active_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!client_ptr_->wait_for_action_server(10s)) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting");
|
||||
is_active_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->node_->get_logger(), "Spin action server is ready");
|
||||
is_active_ = true;
|
||||
}
|
||||
|
||||
void SpinBehaviorTester::deactivate()
|
||||
{
|
||||
if (!is_active_) {
|
||||
throw std::runtime_error("Trying to deactivate while already inactive");
|
||||
}
|
||||
is_active_ = false;
|
||||
}
|
||||
|
||||
bool SpinBehaviorTester::defaultSpinBehaviorTest(
|
||||
const float target_yaw,
|
||||
const double tolerance)
|
||||
{
|
||||
if (!is_active_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Not activated");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Sleep to let behavior server be ready for serving in multiple runs
|
||||
std::this_thread::sleep_for(5s);
|
||||
|
||||
if (make_fake_costmap_) {
|
||||
sendFakeOdom(0.0);
|
||||
}
|
||||
|
||||
auto goal_msg = Spin::Goal();
|
||||
goal_msg.target_yaw = target_yaw;
|
||||
|
||||
// Initialize fake costmap
|
||||
if (make_fake_costmap_) {
|
||||
sendFakeCostmap(target_yaw);
|
||||
sendFakeOdom(0.0);
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped initial_pose;
|
||||
if (!nav2_util::getCurrentPose(initial_pose, *tf_buffer_, "odom")) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
|
||||
return false;
|
||||
}
|
||||
RCLCPP_INFO(node_->get_logger(), "Found current robot pose");
|
||||
RCLCPP_INFO(
|
||||
node_->get_logger(),
|
||||
"Init Yaw is %lf",
|
||||
fabs(tf2::getYaw(initial_pose.pose.orientation)));
|
||||
RCLCPP_INFO(node_->get_logger(), "Before sending goal");
|
||||
|
||||
// Initialize fake costmap
|
||||
if (make_fake_costmap_) {
|
||||
sendFakeCostmap(target_yaw);
|
||||
sendFakeOdom(0.0);
|
||||
}
|
||||
|
||||
rclcpp::sleep_for(std::chrono::milliseconds(100));
|
||||
|
||||
auto goal_handle_future = client_ptr_->async_send_goal(goal_msg);
|
||||
|
||||
if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp_action::ClientGoalHandle<Spin>::SharedPtr goal_handle = goal_handle_future.get();
|
||||
if (!goal_handle) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Wait for the server to be done with the goal
|
||||
auto result_future = client_ptr_->async_get_result(goal_handle);
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for result");
|
||||
rclcpp::sleep_for(std::chrono::milliseconds(1000));
|
||||
|
||||
if (make_fake_costmap_) { // if we are faking the costmap, we will fake success.
|
||||
sendFakeOdom(0.0);
|
||||
sendFakeCostmap(target_yaw);
|
||||
RCLCPP_INFO(node_->get_logger(), "target_yaw %lf", target_yaw);
|
||||
// Slowly increment command yaw by increment to simulate the robot slowly spinning into place
|
||||
float step_size = tolerance / 4.0;
|
||||
for (float command_yaw = 0.0;
|
||||
abs(command_yaw) < abs(target_yaw);
|
||||
command_yaw = command_yaw + step_size)
|
||||
{
|
||||
sendFakeOdom(command_yaw);
|
||||
sendFakeCostmap(target_yaw);
|
||||
rclcpp::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
sendFakeOdom(target_yaw);
|
||||
sendFakeCostmap(target_yaw);
|
||||
RCLCPP_INFO(node_->get_logger(), "After sending goal");
|
||||
}
|
||||
if (rclcpp::spin_until_future_complete(node_, result_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "get result call failed :(");
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp_action::ClientGoalHandle<Spin>::WrappedResult wrapped_result = result_future.get();
|
||||
|
||||
switch (wrapped_result.code) {
|
||||
case rclcpp_action::ResultCode::SUCCEEDED: break;
|
||||
case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal was aborted");
|
||||
return false;
|
||||
case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal was canceled");
|
||||
return false;
|
||||
default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
|
||||
return false;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "result received");
|
||||
|
||||
geometry_msgs::msg::PoseStamped current_pose;
|
||||
if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
|
||||
return false;
|
||||
}
|
||||
|
||||
double goal_yaw = angles::normalize_angle(
|
||||
tf2::getYaw(initial_pose.pose.orientation) + target_yaw);
|
||||
double dyaw = angles::shortest_angular_distance(
|
||||
goal_yaw, tf2::getYaw(current_pose.pose.orientation));
|
||||
|
||||
if (fabs(dyaw) > tolerance) {
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Init Yaw is %lf (tolerance %lf)",
|
||||
fabs(tf2::getYaw(initial_pose.pose.orientation)), tolerance);
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Current Yaw is %lf (tolerance %lf)",
|
||||
fabs(tf2::getYaw(current_pose.pose.orientation)), tolerance);
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Angular distance from goal is %lf (tolerance %lf)",
|
||||
fabs(dyaw), tolerance);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void SpinBehaviorTester::sendFakeCostmap(float angle)
|
||||
{
|
||||
nav2_msgs::msg::Costmap fake_costmap;
|
||||
|
||||
fake_costmap.header.frame_id = "odom";
|
||||
fake_costmap.header.stamp = stamp_;
|
||||
fake_costmap.metadata.layer = "master";
|
||||
fake_costmap.metadata.resolution = .1;
|
||||
fake_costmap.metadata.size_x = 100;
|
||||
fake_costmap.metadata.size_y = 100;
|
||||
fake_costmap.metadata.origin.position.x = 0;
|
||||
fake_costmap.metadata.origin.position.y = 0;
|
||||
fake_costmap.metadata.origin.orientation.w = 1.0;
|
||||
float costmap_val = 0;
|
||||
for (int ix = 0; ix < 100; ix++) {
|
||||
for (int iy = 0; iy < 100; iy++) {
|
||||
if (abs(angle) > M_PI_2f32) {
|
||||
// fake obstacles in the way so we get failure due to potential collision
|
||||
costmap_val = 100;
|
||||
}
|
||||
fake_costmap.data.push_back(costmap_val);
|
||||
}
|
||||
}
|
||||
fake_costmap_publisher_->publish(fake_costmap);
|
||||
}
|
||||
|
||||
void SpinBehaviorTester::sendInitialPose()
|
||||
{
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped pose;
|
||||
pose.header.frame_id = "map";
|
||||
pose.header.stamp = stamp_;
|
||||
pose.pose.pose.position.x = -2.0;
|
||||
pose.pose.pose.position.y = -0.5;
|
||||
pose.pose.pose.position.z = 0.0;
|
||||
pose.pose.pose.orientation.x = 0.0;
|
||||
pose.pose.pose.orientation.y = 0.0;
|
||||
pose.pose.pose.orientation.z = 0.0;
|
||||
pose.pose.pose.orientation.w = 1.0;
|
||||
for (int i = 0; i < 35; i++) {
|
||||
pose.pose.covariance[i] = 0.0;
|
||||
}
|
||||
pose.pose.covariance[0] = 0.08;
|
||||
pose.pose.covariance[7] = 0.08;
|
||||
pose.pose.covariance[35] = 0.05;
|
||||
|
||||
publisher_->publish(pose);
|
||||
RCLCPP_INFO(node_->get_logger(), "Sent initial pose");
|
||||
}
|
||||
|
||||
void SpinBehaviorTester::sendFakeOdom(float angle)
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped transformStamped;
|
||||
|
||||
transformStamped.header.stamp = stamp_;
|
||||
transformStamped.header.frame_id = "odom";
|
||||
transformStamped.child_frame_id = "base_link";
|
||||
transformStamped.transform.translation.x = 0.0;
|
||||
transformStamped.transform.translation.y = 0.0;
|
||||
transformStamped.transform.translation.z = 0.0;
|
||||
tf2::Quaternion q;
|
||||
q.setRPY(0, 0, angle);
|
||||
transformStamped.transform.rotation.x = q.x();
|
||||
transformStamped.transform.rotation.y = q.y();
|
||||
transformStamped.transform.rotation.z = q.z();
|
||||
transformStamped.transform.rotation.w = q.w();
|
||||
|
||||
tf_broadcaster_->sendTransform(transformStamped);
|
||||
|
||||
geometry_msgs::msg::PolygonStamped footprint;
|
||||
footprint.header.frame_id = "odom";
|
||||
footprint.header.stamp = stamp_;
|
||||
footprint.polygon.points.resize(4);
|
||||
footprint.polygon.points[0].x = 0.22;
|
||||
footprint.polygon.points[0].y = 0.22;
|
||||
footprint.polygon.points[1].x = 0.22;
|
||||
footprint.polygon.points[1].y = -0.22;
|
||||
footprint.polygon.points[2].x = -0.22;
|
||||
footprint.polygon.points[2].y = -0.22;
|
||||
footprint.polygon.points[3].x = -0.22;
|
||||
footprint.polygon.points[3].y = 0.22;
|
||||
fake_footprint_publisher_->publish(footprint);
|
||||
}
|
||||
void SpinBehaviorTester::amclPoseCallback(
|
||||
const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr)
|
||||
{
|
||||
initial_pose_received_ = true;
|
||||
}
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
@@ -0,0 +1,108 @@
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_
|
||||
#define BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <algorithm>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "angles/angles.h"
|
||||
#include "nav2_msgs/action/spin.hpp"
|
||||
#include "nav2_msgs/msg/costmap.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "nav2_util/node_thread.hpp"
|
||||
#include "geometry_msgs/msg/point32.hpp"
|
||||
#include "geometry_msgs/msg/polygon_stamped.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
|
||||
#include "geometry_msgs/msg/transform_stamped.hpp"
|
||||
#include "geometry_msgs/msg/quaternion.hpp"
|
||||
|
||||
#include "tf2/utils.h"
|
||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
class SpinBehaviorTester
|
||||
{
|
||||
public:
|
||||
using Spin = nav2_msgs::action::Spin;
|
||||
using GoalHandleSpin = rclcpp_action::ClientGoalHandle<Spin>;
|
||||
|
||||
SpinBehaviorTester();
|
||||
~SpinBehaviorTester();
|
||||
|
||||
// Runs a single test with given target yaw
|
||||
bool defaultSpinBehaviorTest(
|
||||
float target_yaw,
|
||||
double tolerance = 0.1);
|
||||
|
||||
void activate();
|
||||
|
||||
void deactivate();
|
||||
|
||||
bool isActive() const
|
||||
{
|
||||
return is_active_;
|
||||
}
|
||||
|
||||
private:
|
||||
void sendInitialPose();
|
||||
|
||||
void sendFakeCostmap(float angle);
|
||||
void sendFakeOdom(float angle);
|
||||
|
||||
void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
|
||||
|
||||
bool is_active_;
|
||||
bool initial_pose_received_;
|
||||
bool make_fake_costmap_;
|
||||
rclcpp::Time stamp_;
|
||||
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
// Publisher to publish initial pose
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
|
||||
|
||||
// Publisher to publish fake costmap raw
|
||||
rclcpp::Publisher<nav2_msgs::msg::Costmap>::SharedPtr fake_costmap_publisher_;
|
||||
|
||||
// Publisher to publish fake costmap footprint
|
||||
rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr fake_footprint_publisher_;
|
||||
|
||||
// Subscriber for amcl pose
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
|
||||
|
||||
// Action client to call spin action
|
||||
rclcpp_action::Client<Spin>::SharedPtr client_ptr_;
|
||||
};
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
|
||||
#endif // BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_
|
||||
+161
@@ -0,0 +1,161 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright (c) 2019 Samsung Research America
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import os
|
||||
import sys
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch import LaunchService
|
||||
from launch.actions import DeclareLaunchArgument, ExecuteProcess, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
autostart = LaunchConfiguration('autostart')
|
||||
params_file = LaunchConfiguration('params_file')
|
||||
default_nav_through_poses_bt_xml = LaunchConfiguration('default_nav_through_poses_bt_xml')
|
||||
default_nav_to_pose_bt_xml = LaunchConfiguration('default_nav_to_pose_bt_xml')
|
||||
map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local')
|
||||
|
||||
# Create our own temporary YAML files that include substitutions
|
||||
param_substitutions = {
|
||||
'use_sim_time': use_sim_time,
|
||||
'default_nav_through_poses_bt_xml': default_nav_through_poses_bt_xml,
|
||||
'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml,
|
||||
'autostart': autostart,
|
||||
'map_subscribe_transient_local': map_subscribe_transient_local}
|
||||
|
||||
configured_params = RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key=namespace,
|
||||
param_rewrites=param_substitutions,
|
||||
convert_types=True)
|
||||
|
||||
lifecycle_nodes = ['behavior_server']
|
||||
|
||||
# Map fully qualified names to relative ones so the node's namespace can be prepended.
|
||||
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
|
||||
# https://github.com/ros/geometry2/issues/32
|
||||
# https://github.com/ros/robot_state_publisher/pull/30
|
||||
# TODO(orduno) Substitute with `PushNodeRemapping`
|
||||
# https://github.com/ros2/launch_ros/issues/56
|
||||
remappings = [('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static')]
|
||||
|
||||
return LaunchDescription([
|
||||
SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
|
||||
SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'namespace', default_value='',
|
||||
description='Top-level namespace'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'autostart', default_value='true',
|
||||
description='Automatically startup the nav2 stack'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
|
||||
description='Full path to the ROS2 parameters file to use'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'default_nav_through_poses_bt_xml',
|
||||
default_value=os.path.join(
|
||||
get_package_share_directory('nav2_bt_navigator'),
|
||||
'behavior_trees', 'navigate_through_poses_w_replanning_and_recovery.xml'),
|
||||
description='Full path to the behavior tree xml file to use'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'default_nav_to_pose_bt_xml',
|
||||
default_value=os.path.join(
|
||||
get_package_share_directory('nav2_bt_navigator'),
|
||||
'behavior_trees', 'navigate_to_pose_w_replanning_and_recovery.xml'),
|
||||
description='Full path to the behavior tree xml file to use'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'map_subscribe_transient_local', default_value='false',
|
||||
description='Whether to set the map subscriber QoS to transient local'),
|
||||
|
||||
# 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', 'map', 'odom']),
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
|
||||
|
||||
Node(
|
||||
package='nav2_behaviors',
|
||||
executable='behavior_server',
|
||||
name='behavior_server',
|
||||
output='screen',
|
||||
parameters=[configured_params],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_navigation',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': use_sim_time},
|
||||
{'autostart': autostart},
|
||||
{'node_names': lifecycle_nodes}]),
|
||||
])
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
ld = generate_launch_description()
|
||||
|
||||
testExecutable = os.getenv('TEST_EXECUTABLE')
|
||||
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[testExecutable],
|
||||
name='test_spin_behavior_fake_node',
|
||||
output='screen')
|
||||
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
+102
@@ -0,0 +1,102 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright (c) 2019 Samsung Research America
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import os
|
||||
import sys
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch import LaunchService
|
||||
from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
map_yaml_file = os.getenv('TEST_MAP')
|
||||
world = os.getenv('TEST_WORLD')
|
||||
|
||||
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')
|
||||
|
||||
# Replace the `use_astar` setting on the params file
|
||||
configured_params = RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key='',
|
||||
param_rewrites='',
|
||||
convert_types=True)
|
||||
|
||||
return LaunchDescription([
|
||||
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'),
|
||||
|
||||
# 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']),
|
||||
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={
|
||||
'map': map_yaml_file,
|
||||
'use_sim_time': 'True',
|
||||
'params_file': configured_params,
|
||||
'bt_xml_file': bt_navigator_xml,
|
||||
'autostart': 'True'}.items()),
|
||||
])
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
ld = generate_launch_description()
|
||||
|
||||
testExecutable = os.getenv('TEST_EXECUTABLE')
|
||||
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[testExecutable], name='test_spin_behavior_node', output='screen',
|
||||
)
|
||||
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,109 @@
|
||||
// Copyright (c) 2020 Samsung Research
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <cmath>
|
||||
#include <tuple>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "spin_behavior_tester.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
using nav2_system_tests::SpinBehaviorTester;
|
||||
|
||||
std::string testNameGenerator(const testing::TestParamInfo<std::tuple<float, float>> & param)
|
||||
{
|
||||
std::string name = std::to_string(std::abs(std::get<0>(param.param))) + "_" + std::to_string(
|
||||
std::get<1>(param.param));
|
||||
name.erase(std::remove(name.begin(), name.end(), '.'), name.end());
|
||||
return name;
|
||||
}
|
||||
|
||||
class SpinBehaviorTestFixture
|
||||
: public ::testing::TestWithParam<std::tuple<float, float>>
|
||||
{
|
||||
public:
|
||||
static void SetUpTestCase()
|
||||
{
|
||||
spin_recovery_tester = new SpinBehaviorTester();
|
||||
if (!spin_recovery_tester->isActive()) {
|
||||
spin_recovery_tester->activate();
|
||||
}
|
||||
}
|
||||
|
||||
static void TearDownTestCase()
|
||||
{
|
||||
delete spin_recovery_tester;
|
||||
spin_recovery_tester = nullptr;
|
||||
}
|
||||
|
||||
protected:
|
||||
static SpinBehaviorTester * spin_recovery_tester;
|
||||
};
|
||||
|
||||
SpinBehaviorTester * SpinBehaviorTestFixture::spin_recovery_tester = nullptr;
|
||||
|
||||
TEST_P(SpinBehaviorTestFixture, testSpinRecovery)
|
||||
{
|
||||
float target_yaw = std::get<0>(GetParam());
|
||||
float tolerance = std::get<1>(GetParam());
|
||||
|
||||
bool success = false;
|
||||
int num_tries = 3;
|
||||
for (int i = 0; i != num_tries; i++) {
|
||||
success = success || spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance);
|
||||
if (success) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(target_yaw) > M_PI_2f32) {
|
||||
// if this variable is set, make a fake costmap
|
||||
// in the fake spin test, we expect a collision for angles > M_PI_2
|
||||
EXPECT_EQ(false, success);
|
||||
} else {
|
||||
EXPECT_EQ(true, success);
|
||||
}
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
SpinRecoveryTests,
|
||||
SpinBehaviorTestFixture,
|
||||
::testing::Values(
|
||||
std::make_tuple(-M_PIf32 / 6.0, 0.1),
|
||||
std::make_tuple(M_PI_4f32, 0.1),
|
||||
std::make_tuple(-M_PI_2f32, 0.1),
|
||||
std::make_tuple(M_PIf32, 0.1),
|
||||
std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15),
|
||||
std::make_tuple(-2.0 * M_PIf32, 0.1),
|
||||
std::make_tuple(4.0 * M_PIf32, 0.15)),
|
||||
testNameGenerator);
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
set(test_wait_behavior_exec test_wait_behavior_node)
|
||||
|
||||
ament_add_gtest_executable(${test_wait_behavior_exec}
|
||||
test_wait_behavior_node.cpp
|
||||
wait_behavior_tester.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${test_wait_behavior_exec}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
ament_add_test(test_wait_behavior
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wait_behavior_launch.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
TIMEOUT 180
|
||||
ENV
|
||||
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
|
||||
TEST_EXECUTABLE=$<TARGET_FILE:${test_wait_behavior_exec}>
|
||||
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_behavior.xml
|
||||
)
|
||||
+102
@@ -0,0 +1,102 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright (c) 2019 Samsung Research America
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import os
|
||||
import sys
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch import LaunchService
|
||||
from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
map_yaml_file = os.getenv('TEST_MAP')
|
||||
world = os.getenv('TEST_WORLD')
|
||||
|
||||
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')
|
||||
|
||||
# Replace the `use_astar` setting on the params file
|
||||
configured_params = RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key='',
|
||||
param_rewrites='',
|
||||
convert_types=True)
|
||||
|
||||
return LaunchDescription([
|
||||
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'),
|
||||
|
||||
# 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']),
|
||||
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={
|
||||
'map': map_yaml_file,
|
||||
'use_sim_time': 'True',
|
||||
'params_file': configured_params,
|
||||
'bt_xml_file': bt_navigator_xml,
|
||||
'autostart': 'True'}.items()),
|
||||
])
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
ld = generate_launch_description()
|
||||
|
||||
testExecutable = os.getenv('TEST_EXECUTABLE')
|
||||
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[testExecutable], name='test_wait_behavior_node', output='screen',
|
||||
)
|
||||
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,106 @@
|
||||
// Copyright (c) 2020 Samsung Research
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <cmath>
|
||||
#include <tuple>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "wait_behavior_tester.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
using nav2_system_tests::WaitBehaviorTester;
|
||||
|
||||
std::string testNameGenerator(const testing::TestParamInfo<std::tuple<float, float>> & param)
|
||||
{
|
||||
std::string name = std::to_string(std::abs(std::get<0>(param.param))) + "_" + std::to_string(
|
||||
std::get<1>(param.param));
|
||||
name.erase(std::remove(name.begin(), name.end(), '.'), name.end());
|
||||
return name;
|
||||
}
|
||||
|
||||
class WaitBehaviorTestFixture
|
||||
: public ::testing::TestWithParam<std::tuple<float, float>>
|
||||
{
|
||||
public:
|
||||
static void SetUpTestCase()
|
||||
{
|
||||
wait_behavior_tester = new WaitBehaviorTester();
|
||||
if (!wait_behavior_tester->isActive()) {
|
||||
wait_behavior_tester->activate();
|
||||
}
|
||||
}
|
||||
|
||||
static void TearDownTestCase()
|
||||
{
|
||||
delete wait_behavior_tester;
|
||||
wait_behavior_tester = nullptr;
|
||||
}
|
||||
|
||||
protected:
|
||||
static WaitBehaviorTester * wait_behavior_tester;
|
||||
};
|
||||
|
||||
WaitBehaviorTester * WaitBehaviorTestFixture::wait_behavior_tester = nullptr;
|
||||
|
||||
TEST_P(WaitBehaviorTestFixture, testSWaitBehavior)
|
||||
{
|
||||
float wait_time = std::get<0>(GetParam());
|
||||
float cancel = std::get<1>(GetParam());
|
||||
|
||||
bool success = false;
|
||||
int num_tries = 3;
|
||||
for (int i = 0; i != num_tries; i++) {
|
||||
if (cancel == 1.0) {
|
||||
success = success || wait_behavior_tester->behaviorTestCancel(wait_time);
|
||||
} else {
|
||||
success = success || wait_behavior_tester->behaviorTest(wait_time);
|
||||
}
|
||||
if (success) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
EXPECT_EQ(true, success);
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
WaitBehaviorTests,
|
||||
WaitBehaviorTestFixture,
|
||||
::testing::Values(
|
||||
std::make_tuple(1.0, 0.0),
|
||||
std::make_tuple(2.0, 0.0),
|
||||
std::make_tuple(5.0, 0.0),
|
||||
std::make_tuple(10.0, 1.0)),
|
||||
testNameGenerator);
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,280 @@
|
||||
// Copyright (c) 2020 Samsung Research
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <string>
|
||||
#include <random>
|
||||
#include <tuple>
|
||||
#include <memory>
|
||||
#include <iostream>
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
#include <iomanip>
|
||||
|
||||
#include "wait_behavior_tester.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using namespace std::chrono; // NOLINT
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
WaitBehaviorTester::WaitBehaviorTester()
|
||||
: is_active_(false),
|
||||
initial_pose_received_(false)
|
||||
{
|
||||
node_ = rclcpp::Node::make_shared("wait_behavior_test");
|
||||
|
||||
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
|
||||
client_ptr_ = rclcpp_action::create_client<Wait>(
|
||||
node_->get_node_base_interface(),
|
||||
node_->get_node_graph_interface(),
|
||||
node_->get_node_logging_interface(),
|
||||
node_->get_node_waitables_interface(),
|
||||
"wait");
|
||||
|
||||
publisher_ =
|
||||
node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
|
||||
|
||||
subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
|
||||
"amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
|
||||
std::bind(&WaitBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
WaitBehaviorTester::~WaitBehaviorTester()
|
||||
{
|
||||
if (is_active_) {
|
||||
deactivate();
|
||||
}
|
||||
}
|
||||
|
||||
void WaitBehaviorTester::activate()
|
||||
{
|
||||
if (is_active_) {
|
||||
throw std::runtime_error("Trying to activate while already active");
|
||||
return;
|
||||
}
|
||||
|
||||
while (!initial_pose_received_) {
|
||||
RCLCPP_WARN(node_->get_logger(), "Initial pose not received");
|
||||
sendInitialPose();
|
||||
std::this_thread::sleep_for(100ms);
|
||||
rclcpp::spin_some(node_);
|
||||
}
|
||||
|
||||
// Wait for lifecycle_manager_navigation to activate behavior_server
|
||||
std::this_thread::sleep_for(10s);
|
||||
|
||||
if (!client_ptr_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Action client not initialized");
|
||||
is_active_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!client_ptr_->wait_for_action_server(10s)) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting");
|
||||
is_active_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->node_->get_logger(), "Wait action server is ready");
|
||||
is_active_ = true;
|
||||
}
|
||||
|
||||
void WaitBehaviorTester::deactivate()
|
||||
{
|
||||
if (!is_active_) {
|
||||
throw std::runtime_error("Trying to deactivate while already inactive");
|
||||
}
|
||||
is_active_ = false;
|
||||
}
|
||||
|
||||
bool WaitBehaviorTester::behaviorTest(
|
||||
const float wait_time)
|
||||
{
|
||||
if (!is_active_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Not activated");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Sleep to let behavior server be ready for serving in multiple runs
|
||||
std::this_thread::sleep_for(5s);
|
||||
|
||||
auto start_time = node_->now();
|
||||
auto goal_msg = Wait::Goal();
|
||||
goal_msg.time = rclcpp::Duration(wait_time, 0.0);
|
||||
|
||||
RCLCPP_INFO(this->node_->get_logger(), "Sending goal");
|
||||
|
||||
auto goal_handle_future = client_ptr_->async_send_goal(goal_msg);
|
||||
|
||||
if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp_action::ClientGoalHandle<Wait>::SharedPtr goal_handle = goal_handle_future.get();
|
||||
if (!goal_handle) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Wait for the server to be done with the goal
|
||||
auto result_future = client_ptr_->async_get_result(goal_handle);
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for result");
|
||||
if (rclcpp::spin_until_future_complete(node_, result_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "get result call failed :(");
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp_action::ClientGoalHandle<Wait>::WrappedResult wrapped_result = result_future.get();
|
||||
|
||||
switch (wrapped_result.code) {
|
||||
case rclcpp_action::ResultCode::SUCCEEDED: break;
|
||||
case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal was aborted");
|
||||
return false;
|
||||
case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal was canceled");
|
||||
return false;
|
||||
default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
|
||||
return false;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "result received");
|
||||
|
||||
if ((node_->now() - start_time).seconds() < static_cast<double>(wait_time)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool WaitBehaviorTester::behaviorTestCancel(
|
||||
const float wait_time)
|
||||
{
|
||||
if (!is_active_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Not activated");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Sleep to let behavior server be ready for serving in multiple runs
|
||||
std::this_thread::sleep_for(5s);
|
||||
|
||||
auto start_time = node_->now();
|
||||
auto goal_msg = Wait::Goal();
|
||||
goal_msg.time = rclcpp::Duration(wait_time, 0.0);
|
||||
|
||||
RCLCPP_INFO(this->node_->get_logger(), "Sending goal");
|
||||
|
||||
auto goal_handle_future = client_ptr_->async_send_goal(goal_msg);
|
||||
|
||||
if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
|
||||
return false;
|
||||
}
|
||||
|
||||
rclcpp_action::ClientGoalHandle<Wait>::SharedPtr goal_handle = goal_handle_future.get();
|
||||
if (!goal_handle) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Wait for the server to be done with the goal
|
||||
auto result_future = client_ptr_->async_cancel_all_goals();
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for cancellation");
|
||||
if (rclcpp::spin_until_future_complete(node_, result_future) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_ERROR(node_->get_logger(), "get cancel result call failed :(");
|
||||
return false;
|
||||
}
|
||||
|
||||
auto status = goal_handle_future.get()->get_status();
|
||||
|
||||
switch (status) {
|
||||
case rclcpp_action::GoalStatus::STATUS_SUCCEEDED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal succeeded");
|
||||
return false;
|
||||
case rclcpp_action::GoalStatus::STATUS_ABORTED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal was aborted");
|
||||
return false;
|
||||
case rclcpp_action::GoalStatus::STATUS_CANCELED: RCLCPP_INFO(
|
||||
node_->get_logger(),
|
||||
"Goal was canceled");
|
||||
return true;
|
||||
case rclcpp_action::GoalStatus::STATUS_CANCELING: RCLCPP_INFO(
|
||||
node_->get_logger(),
|
||||
"Goal is cancelling");
|
||||
return true;
|
||||
case rclcpp_action::GoalStatus::STATUS_EXECUTING: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal is executing");
|
||||
return false;
|
||||
case rclcpp_action::GoalStatus::STATUS_ACCEPTED: RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"Goal is processing");
|
||||
return false;
|
||||
default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
|
||||
return false;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void WaitBehaviorTester::sendInitialPose()
|
||||
{
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped pose;
|
||||
pose.header.frame_id = "map";
|
||||
pose.header.stamp = rclcpp::Time();
|
||||
pose.pose.pose.position.x = -2.0;
|
||||
pose.pose.pose.position.y = -0.5;
|
||||
pose.pose.pose.position.z = 0.0;
|
||||
pose.pose.pose.orientation.x = 0.0;
|
||||
pose.pose.pose.orientation.y = 0.0;
|
||||
pose.pose.pose.orientation.z = 0.0;
|
||||
pose.pose.pose.orientation.w = 1.0;
|
||||
for (int i = 0; i < 35; i++) {
|
||||
pose.pose.covariance[i] = 0.0;
|
||||
}
|
||||
pose.pose.covariance[0] = 0.08;
|
||||
pose.pose.covariance[7] = 0.08;
|
||||
pose.pose.covariance[35] = 0.05;
|
||||
|
||||
publisher_->publish(pose);
|
||||
RCLCPP_INFO(node_->get_logger(), "Sent initial pose");
|
||||
}
|
||||
|
||||
void WaitBehaviorTester::amclPoseCallback(
|
||||
const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr)
|
||||
{
|
||||
initial_pose_received_ = true;
|
||||
}
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
@@ -0,0 +1,91 @@
|
||||
// Copyright (c) 2020 Samsung Research
|
||||
// Copyright (c) 2020 Sarthak Mittal
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#ifndef BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_
|
||||
#define BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <algorithm>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "angles/angles.h"
|
||||
#include "nav2_msgs/action/wait.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "nav2_util/node_thread.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
|
||||
|
||||
#include "tf2/utils.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
class WaitBehaviorTester
|
||||
{
|
||||
public:
|
||||
using Wait = nav2_msgs::action::Wait;
|
||||
using GoalHandleWait = rclcpp_action::ClientGoalHandle<Wait>;
|
||||
|
||||
WaitBehaviorTester();
|
||||
~WaitBehaviorTester();
|
||||
|
||||
// Runs a single test with given target yaw
|
||||
bool behaviorTest(
|
||||
float time);
|
||||
|
||||
bool behaviorTestCancel(float time);
|
||||
|
||||
void activate();
|
||||
|
||||
void deactivate();
|
||||
|
||||
bool isActive() const
|
||||
{
|
||||
return is_active_;
|
||||
}
|
||||
|
||||
private:
|
||||
void sendInitialPose();
|
||||
|
||||
void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
|
||||
|
||||
bool is_active_;
|
||||
bool initial_pose_received_;
|
||||
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
// Publisher to publish initial pose
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
|
||||
|
||||
// Subscriber for amcl pose
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
|
||||
|
||||
// Action client to call wait action
|
||||
rclcpp_action::Client<Wait>::SharedPtr client_ptr_;
|
||||
};
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
|
||||
#endif // BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_
|
||||
@@ -0,0 +1,47 @@
|
||||
ament_add_test(test_keepout_filter
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_keepout_launch.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
TIMEOUT 180
|
||||
ENV
|
||||
TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
|
||||
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
|
||||
TEST_MASK=${PROJECT_SOURCE_DIR}/maps/keepout_mask.yaml
|
||||
TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
|
||||
PARAMS_FILE=${CMAKE_CURRENT_SOURCE_DIR}/keepout_params.yaml
|
||||
GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
|
||||
BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
|
||||
ASTAR=False
|
||||
)
|
||||
|
||||
ament_add_test(test_speed_filter_global
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_speed_launch.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
TIMEOUT 180
|
||||
ENV
|
||||
TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
|
||||
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
|
||||
TEST_MASK=${PROJECT_SOURCE_DIR}/maps/speed_mask.yaml
|
||||
TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
|
||||
PARAMS_FILE=${CMAKE_CURRENT_SOURCE_DIR}/speed_global_params.yaml
|
||||
GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
|
||||
BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
|
||||
ASTAR=False
|
||||
)
|
||||
|
||||
ament_add_test(test_speed_filter_local
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_speed_launch.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
TIMEOUT 180
|
||||
ENV
|
||||
TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
|
||||
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
|
||||
TEST_MASK=${PROJECT_SOURCE_DIR}/maps/speed_mask.yaml
|
||||
TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
|
||||
PARAMS_FILE=${CMAKE_CURRENT_SOURCE_DIR}/speed_local_params.yaml
|
||||
GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
|
||||
BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
|
||||
ASTAR=False
|
||||
)
|
||||
@@ -0,0 +1,333 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_footprint"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: true
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field_prob"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.05
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::OmniMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 1.0
|
||||
update_min_a: 0.2
|
||||
update_min_d: 0.25
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: scan
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are set in the launch
|
||||
# files to allow for a commandline change default used is the
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml &
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["goal_checker"]
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
goal_checker:
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
stateful: True
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
prune_distance: 1.0
|
||||
forward_prune_distance: 1.0
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.26
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.26
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
publish_cost_grid_pc: True
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
filters: ["keepout_filter"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
map_subscribe_transient_local: True
|
||||
keepout_filter:
|
||||
plugin: "nav2_costmap_2d::KeepoutFilter"
|
||||
enabled: True
|
||||
filter_info_topic: "/costmap_filter_info"
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
filters: ["keepout_filter"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
keepout_filter:
|
||||
plugin: "nav2_costmap_2d::KeepoutFilter"
|
||||
enabled: True
|
||||
filter_info_topic: "/costmap_filter_info"
|
||||
always_send_full_costmap: True
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
yaml_filename: "turtlebot3_world.yaml"
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: False
|
||||
allow_unknown: True
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "waypoint_task_executor"
|
||||
waypoint_task_executor:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 0
|
||||
|
||||
costmap_filter_info_server:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
type: 0
|
||||
filter_info_topic: "/costmap_filter_info"
|
||||
mask_topic: "/filter_mask"
|
||||
base: 0.0
|
||||
multiplier: 1.0
|
||||
|
||||
filter_mask_server:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
frame_id: "map"
|
||||
topic_name: "/filter_mask"
|
||||
yaml_filename: "keepout_mask.yaml"
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 104 KiB |
@@ -0,0 +1,324 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_footprint"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "beam"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.05
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 1.0
|
||||
update_min_a: 0.2
|
||||
update_min_d: 0.25
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: scan
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are set in the launch
|
||||
# files to allow for a commandline change default used is the
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml &
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
speed_limit_topic: "/speed_limit"
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["goal_checker"]
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
goal_checker:
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
stateful: True
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.26
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.26
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
static_layer:
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
filters: ["speed_filter"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
speed_filter:
|
||||
plugin: "nav2_costmap_2d::SpeedFilter"
|
||||
enabled: True
|
||||
filter_info_topic: "/costmap_filter_info"
|
||||
speed_limit_topic: "/speed_limit"
|
||||
always_send_full_costmap: True
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
yaml_filename: "turtlebot3_world.yaml"
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: False
|
||||
allow_unknown: True
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "waypoint_task_executor"
|
||||
waypoint_task_executor:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 0
|
||||
|
||||
costmap_filter_info_server:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
type: 1
|
||||
filter_info_topic: "/costmap_filter_info"
|
||||
mask_topic: "/filter_mask"
|
||||
base: 100.0
|
||||
multiplier: -1.0
|
||||
|
||||
filter_mask_server:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
frame_id: "map"
|
||||
topic_name: "/filter_mask"
|
||||
yaml_filename: "speed_mask.yaml"
|
||||
@@ -0,0 +1,324 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_footprint"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "beam"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.05
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 1.0
|
||||
update_min_a: 0.2
|
||||
update_min_d: 0.25
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: scan
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are set in the launch
|
||||
# files to allow for a commandline change default used is the
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml &
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
speed_limit_topic: "/speed_limit"
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["goal_checker"]
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
goal_checker:
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
stateful: True
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.26
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.26
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
filters: ["speed_filter"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
static_layer:
|
||||
map_subscribe_transient_local: True
|
||||
speed_filter:
|
||||
plugin: "nav2_costmap_2d::SpeedFilter"
|
||||
enabled: True
|
||||
filter_info_topic: "/costmap_filter_info"
|
||||
speed_limit_topic: "/speed_limit"
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: True
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
yaml_filename: "turtlebot3_world.yaml"
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: False
|
||||
allow_unknown: True
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "waypoint_task_executor"
|
||||
waypoint_task_executor:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 0
|
||||
|
||||
costmap_filter_info_server:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
type: 1
|
||||
filter_info_topic: "/costmap_filter_info"
|
||||
mask_topic: "/filter_mask"
|
||||
base: 100.0
|
||||
multiplier: -1.0
|
||||
|
||||
filter_mask_server:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
frame_id: "map"
|
||||
topic_name: "/filter_mask"
|
||||
yaml_filename: "speed_mask.yaml"
|
||||
@@ -0,0 +1,149 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# Copyright (c) 2018 Intel Corporation
|
||||
# Copyright (c) 2020 Samsung Research Russia
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import os
|
||||
import sys
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch import LaunchService
|
||||
from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
|
||||
from launch.launch_context import LaunchContext
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
map_yaml_file = os.getenv('TEST_MAP')
|
||||
filter_mask_file = os.getenv('TEST_MASK')
|
||||
world = os.getenv('TEST_WORLD')
|
||||
|
||||
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')
|
||||
script_dir = os.path.dirname(os.path.realpath(__file__))
|
||||
params_file = os.path.join(script_dir, 'keepout_params.yaml')
|
||||
|
||||
# Replace the `use_astar` setting on the params file
|
||||
param_substitutions = {
|
||||
'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'),
|
||||
'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file,
|
||||
'map_server.ros__parameters.yaml_filename': map_yaml_file}
|
||||
configured_params = RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key='',
|
||||
param_rewrites=param_substitutions,
|
||||
convert_types=True)
|
||||
|
||||
context = LaunchContext()
|
||||
new_yaml = configured_params.perform(context)
|
||||
return LaunchDescription([
|
||||
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'),
|
||||
|
||||
# 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']),
|
||||
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
|
||||
|
||||
Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_filters',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'node_names':
|
||||
[
|
||||
'filter_mask_server', 'costmap_filter_info_server'
|
||||
]
|
||||
},
|
||||
{'autostart': True}]),
|
||||
|
||||
# Nodes required for Costmap Filters configuration
|
||||
Node(
|
||||
package='nav2_map_server',
|
||||
executable='map_server',
|
||||
name='filter_mask_server',
|
||||
output='screen',
|
||||
parameters=[new_yaml]),
|
||||
|
||||
Node(
|
||||
package='nav2_map_server',
|
||||
executable='costmap_filter_info_server',
|
||||
name='costmap_filter_info_server',
|
||||
output='screen',
|
||||
parameters=[new_yaml]),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={'namespace': '',
|
||||
'use_namespace': 'False',
|
||||
'map': map_yaml_file,
|
||||
'use_sim_time': 'True',
|
||||
'params_file': new_yaml,
|
||||
'bt_xml_file': bt_navigator_xml,
|
||||
'autostart': 'True'}.items()),
|
||||
|
||||
Node(
|
||||
package='nav2_costmap_2d',
|
||||
executable='nav2_costmap_2d_cloud',
|
||||
name='costmap_2d_cloud',
|
||||
output='screen',
|
||||
remappings=[('voxel_grid', 'local_costmap/voxel_grid')]),
|
||||
])
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
ld = generate_launch_description()
|
||||
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'),
|
||||
'-t', 'keepout', '-r', '-2.0', '-0.5', '0.0', '-0.5'],
|
||||
name='tester_node',
|
||||
output='screen')
|
||||
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,140 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# Copyright (c) 2018 Intel Corporation
|
||||
# Copyright (c) 2020 Samsung Research Russia
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import os
|
||||
import sys
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch import LaunchService
|
||||
from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
|
||||
from launch.launch_context import LaunchContext
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
map_yaml_file = os.getenv('TEST_MAP')
|
||||
filter_mask_file = os.getenv('TEST_MASK')
|
||||
world = os.getenv('TEST_WORLD')
|
||||
|
||||
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.getenv('PARAMS_FILE')
|
||||
|
||||
# Replace the `use_astar` setting on the params file
|
||||
param_substitutions = {
|
||||
'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'),
|
||||
'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file,
|
||||
'map_server.ros__parameters.yaml_filename': map_yaml_file}
|
||||
configured_params = RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key='',
|
||||
param_rewrites=param_substitutions,
|
||||
convert_types=True)
|
||||
context = LaunchContext()
|
||||
new_yaml = configured_params.perform(context)
|
||||
return LaunchDescription([
|
||||
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'),
|
||||
|
||||
# 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']),
|
||||
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
|
||||
|
||||
Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_filters',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'node_names':
|
||||
[
|
||||
'filter_mask_server', 'costmap_filter_info_server'
|
||||
]
|
||||
},
|
||||
{'autostart': True}]),
|
||||
|
||||
# Nodes required for Costmap Filters configuration
|
||||
Node(
|
||||
package='nav2_map_server',
|
||||
executable='map_server',
|
||||
name='filter_mask_server',
|
||||
output='screen',
|
||||
parameters=[new_yaml]),
|
||||
|
||||
Node(
|
||||
package='nav2_map_server',
|
||||
executable='costmap_filter_info_server',
|
||||
name='costmap_filter_info_server',
|
||||
output='screen',
|
||||
parameters=[new_yaml]),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={'namespace': '',
|
||||
'use_namespace': 'False',
|
||||
'map': map_yaml_file,
|
||||
'use_sim_time': 'True',
|
||||
'params_file': new_yaml,
|
||||
'bt_xml_file': bt_navigator_xml,
|
||||
'autostart': 'True'}.items()),
|
||||
])
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
ld = generate_launch_description()
|
||||
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'),
|
||||
'-t', 'speed', '-r', '-2.0', '-0.5', '0.0', '-0.5'],
|
||||
name='tester_node',
|
||||
output='screen')
|
||||
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,537 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# Copyright (c) 2018 Intel Corporation.
|
||||
# Copyright (c) 2020 Samsung Research Russia
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import argparse
|
||||
from enum import Enum
|
||||
import math
|
||||
import sys
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
from action_msgs.msg import GoalStatus
|
||||
from geometry_msgs.msg import Pose
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from geometry_msgs.msg import PoseWithCovarianceStamped
|
||||
from lifecycle_msgs.srv import GetState
|
||||
from nav2_msgs.action import NavigateToPose
|
||||
from nav2_msgs.msg import SpeedLimit
|
||||
from nav2_msgs.srv import ManageLifecycleNodes
|
||||
from nav_msgs.msg import OccupancyGrid
|
||||
from nav_msgs.msg import Path
|
||||
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
|
||||
from rclpy.qos import QoSProfile
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
|
||||
|
||||
class TestType(Enum):
|
||||
KEEPOUT = 0
|
||||
SPEED = 1
|
||||
|
||||
|
||||
class FilterMask():
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
filter_mask: OccupancyGrid
|
||||
):
|
||||
self.filter_mask = filter_mask
|
||||
|
||||
# Converts world coordinates into filter mask map coordinate.
|
||||
# Returns filter mask map coordinates or (-1, -1) in case
|
||||
# if world coordinates are out of mask bounds.
|
||||
def worldToMap(self, wx: float, wy: float):
|
||||
origin_x = self.filter_mask.info.origin.position.x
|
||||
origin_y = self.filter_mask.info.origin.position.y
|
||||
size_x = self.filter_mask.info.width
|
||||
size_y = self.filter_mask.info.height
|
||||
resolution = self.filter_mask.info.resolution
|
||||
|
||||
if wx < origin_x or wy < origin_y:
|
||||
return -1, -1
|
||||
|
||||
mx = int((wx - origin_x) / resolution)
|
||||
my = int((wy - origin_y) / resolution)
|
||||
|
||||
if mx < size_x and my < size_y:
|
||||
return mx, my
|
||||
|
||||
return -1, -1
|
||||
|
||||
# Gets filter_mask[mx, my] value
|
||||
def getValue(self, mx, my):
|
||||
size_x = self.filter_mask.info.width
|
||||
return self.filter_mask.data[mx + my * size_x]
|
||||
|
||||
|
||||
class NavTester(Node):
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
test_type: TestType,
|
||||
initial_pose: Pose,
|
||||
goal_pose: Pose,
|
||||
namespace: str = ''
|
||||
):
|
||||
super().__init__(node_name='nav2_tester', namespace=namespace)
|
||||
self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped,
|
||||
'initialpose', 10)
|
||||
self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10)
|
||||
|
||||
transient_local_qos = QoSProfile(
|
||||
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
|
||||
reliability=QoSReliabilityPolicy.RELIABLE,
|
||||
history=QoSHistoryPolicy.KEEP_LAST,
|
||||
depth=1)
|
||||
|
||||
volatile_qos = QoSProfile(
|
||||
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
|
||||
reliability=QoSReliabilityPolicy.RELIABLE,
|
||||
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
|
||||
depth=1)
|
||||
|
||||
self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
|
||||
'amcl_pose', self.poseCallback,
|
||||
transient_local_qos)
|
||||
self.clearing_ep_sub = self.create_subscription(PointCloud2,
|
||||
'local_costmap/clearing_endpoints',
|
||||
self.clearingEndpointsCallback,
|
||||
transient_local_qos)
|
||||
self.test_type = test_type
|
||||
self.filter_test_result = True
|
||||
self.clearing_endpoints_received = False
|
||||
self.voxel_marked_received = False
|
||||
self.voxel_unknown_received = False
|
||||
self.cost_cloud_received = False
|
||||
|
||||
if self.test_type == TestType.KEEPOUT:
|
||||
self.plan_sub = self.create_subscription(Path, 'plan',
|
||||
self.planCallback, volatile_qos)
|
||||
self.voxel_marked_sub = self.create_subscription(PointCloud2,
|
||||
'voxel_marked_cloud',
|
||||
self.voxelMarkedCallback,
|
||||
1)
|
||||
self.voxel_unknown_sub = self.create_subscription(PointCloud2,
|
||||
'voxel_unknown_cloud',
|
||||
self.voxelUnknownCallback,
|
||||
1)
|
||||
self.cost_cloud_sub = self.create_subscription(PointCloud2,
|
||||
'cost_cloud',
|
||||
self.dwbCostCloudCallback,
|
||||
1)
|
||||
elif self.test_type == TestType.SPEED:
|
||||
self.speed_it = 0
|
||||
# Expected chain of speed limits
|
||||
self.limits = [50.0, 0.0]
|
||||
# Permissive array: all received speed limits must match to "limits" from above
|
||||
self.limit_passed = [False, False]
|
||||
self.plan_sub = self.create_subscription(SpeedLimit, 'speed_limit',
|
||||
self.speedLimitCallback, volatile_qos)
|
||||
|
||||
self.mask_received = False
|
||||
self.mask_sub = self.create_subscription(OccupancyGrid, 'filter_mask',
|
||||
self.maskCallback, transient_local_qos)
|
||||
|
||||
self.initial_pose_received = False
|
||||
self.initial_pose = initial_pose
|
||||
self.goal_pose = goal_pose
|
||||
self.action_client = ActionClient(
|
||||
self, NavigateToPose, 'navigate_to_pose')
|
||||
|
||||
def info_msg(self, msg: str):
|
||||
self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
|
||||
|
||||
def warn_msg(self, msg: str):
|
||||
self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
|
||||
|
||||
def error_msg(self, msg: str):
|
||||
self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
|
||||
|
||||
def setInitialPose(self):
|
||||
msg = PoseWithCovarianceStamped()
|
||||
msg.pose.pose = self.initial_pose
|
||||
msg.header.frame_id = 'map'
|
||||
self.info_msg('Publishing Initial Pose')
|
||||
self.initial_pose_pub.publish(msg)
|
||||
self.currentPose = self.initial_pose
|
||||
|
||||
def getStampedPoseMsg(self, pose: Pose):
|
||||
msg = PoseStamped()
|
||||
msg.header.frame_id = 'map'
|
||||
msg.pose = pose
|
||||
return msg
|
||||
|
||||
def publishGoalPose(self, goal_pose: Optional[Pose] = None):
|
||||
self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
|
||||
self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose))
|
||||
|
||||
def runNavigateAction(self, goal_pose: Optional[Pose] = None):
|
||||
# Sends a `NavToPose` action request and waits for completion
|
||||
self.info_msg("Waiting for 'NavigateToPose' action server")
|
||||
while not self.action_client.wait_for_server(timeout_sec=1.0):
|
||||
self.info_msg(
|
||||
"'NavigateToPose' action server not available, waiting...")
|
||||
|
||||
self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
|
||||
goal_msg = NavigateToPose.Goal()
|
||||
goal_msg.pose = self.getStampedPoseMsg(self.goal_pose)
|
||||
|
||||
self.info_msg('Sending goal request...')
|
||||
send_goal_future = self.action_client.send_goal_async(goal_msg)
|
||||
|
||||
rclpy.spin_until_future_complete(self, send_goal_future)
|
||||
goal_handle = send_goal_future.result()
|
||||
|
||||
if not goal_handle.accepted:
|
||||
self.error_msg('Goal rejected')
|
||||
return False
|
||||
|
||||
self.info_msg('Goal accepted')
|
||||
get_result_future = goal_handle.get_result_async()
|
||||
|
||||
self.info_msg("Waiting for 'NavigateToPose' action to complete")
|
||||
rclpy.spin_until_future_complete(self, get_result_future)
|
||||
status = get_result_future.result().status
|
||||
if status != GoalStatus.STATUS_SUCCEEDED:
|
||||
self.info_msg(f'Goal failed with status code: {status}')
|
||||
return False
|
||||
|
||||
self.info_msg('Goal succeeded!')
|
||||
return True
|
||||
|
||||
def isInKeepout(self, x, y):
|
||||
mx, my = self.filter_mask.worldToMap(x, y)
|
||||
if mx == -1 and my == -1: # Out of mask's area
|
||||
return False
|
||||
if self.filter_mask.getValue(mx, my) == 100: # Occupied
|
||||
return True
|
||||
return False
|
||||
|
||||
# Checks that (x, y) position does not belong to a keepout zone.
|
||||
def checkKeepout(self, x, y):
|
||||
if not self.mask_received:
|
||||
self.warn_msg('Filter mask was not received')
|
||||
elif self.isInKeepout(x, y):
|
||||
self.filter_test_result = False
|
||||
self.error_msg(f'Pose ({x}, {y}) belongs to keepout zone')
|
||||
return False
|
||||
return True
|
||||
|
||||
# Checks that currently received speed_limit is equal to the it-th item
|
||||
# of expected speed "limits" array.
|
||||
# If so, sets it-th item of permissive array "limit_passed" to be true.
|
||||
# Otherwise it will be remained to be false.
|
||||
# Also verifies that speed limit messages received no more than N-times
|
||||
# (where N - is the length of "limits" array),
|
||||
# otherwise sets overall "filter_test_result" to be false.
|
||||
def checkSpeed(self, it, speed_limit):
|
||||
if it >= len(self.limits):
|
||||
self.error_msg('Got excess speed limit')
|
||||
self.filter_test_result = False
|
||||
return
|
||||
if speed_limit == self.limits[it]:
|
||||
self.limit_passed[it] = True
|
||||
else:
|
||||
self.error_msg('Incorrect speed limit received: ' + str(speed_limit) +
|
||||
', but should be: ' + str(self.limits[it]))
|
||||
|
||||
def poseCallback(self, msg):
|
||||
self.info_msg('Received amcl_pose')
|
||||
self.current_pose = msg.pose.pose
|
||||
self.initial_pose_received = True
|
||||
if self.test_type == TestType.KEEPOUT:
|
||||
if not self.checkKeepout(msg.pose.pose.position.x, msg.pose.pose.position.y):
|
||||
self.error_msg('Robot goes into keepout zone')
|
||||
|
||||
def planCallback(self, msg):
|
||||
self.info_msg('Received plan')
|
||||
for pose in msg.poses:
|
||||
if not self.checkKeepout(pose.pose.position.x, pose.pose.position.y):
|
||||
self.error_msg('Path plan intersects with keepout zone')
|
||||
return
|
||||
|
||||
def clearingEndpointsCallback(self, msg):
|
||||
if len(msg.data) > 0:
|
||||
self.clearing_endpoints_received = True
|
||||
|
||||
def voxelMarkedCallback(self, msg):
|
||||
if len(msg.data) > 0:
|
||||
self.voxel_marked_received = True
|
||||
|
||||
def voxelUnknownCallback(self, msg):
|
||||
if len(msg.data) > 0:
|
||||
self.voxel_unknown_received = True
|
||||
|
||||
def dwbCostCloudCallback(self, msg):
|
||||
self.info_msg('Received cost_cloud points')
|
||||
if len(msg.data) > 0:
|
||||
self.cost_cloud_received = True
|
||||
|
||||
def speedLimitCallback(self, msg):
|
||||
self.info_msg(f'Received speed limit: {msg.speed_limit}')
|
||||
self.checkSpeed(self.speed_it, msg.speed_limit)
|
||||
self.speed_it += 1
|
||||
|
||||
def maskCallback(self, msg):
|
||||
self.info_msg('Received filter mask')
|
||||
self.filter_mask = FilterMask(msg)
|
||||
self.mask_received = True
|
||||
|
||||
def wait_for_filter_mask(self, timeout):
|
||||
start_time = time.time()
|
||||
|
||||
while not self.mask_received:
|
||||
self.info_msg('Waiting for filter mask to be received ...')
|
||||
rclpy.spin_once(self, timeout_sec=1)
|
||||
if (time.time() - start_time) > timeout:
|
||||
self.error_msg('Time out to waiting filter mask')
|
||||
return False
|
||||
return True
|
||||
|
||||
def wait_for_pointcloud_subscribers(self, timeout):
|
||||
start_time = time.time()
|
||||
while not self.voxel_unknown_received or not self.voxel_marked_received \
|
||||
or not self.clearing_endpoints_received:
|
||||
self.info_msg(
|
||||
'Waiting for voxel_marked_cloud/voxel_unknown_cloud/\
|
||||
clearing_endpoints msg to be received ...')
|
||||
rclpy.spin_once(self, timeout_sec=1)
|
||||
if (time.time() - start_time) > timeout:
|
||||
self.error_msg(
|
||||
'Time out to waiting for voxel_marked_cloud/voxel_unknown_cloud/\
|
||||
clearing_endpoints msgs')
|
||||
return False
|
||||
return True
|
||||
|
||||
def reachesGoal(self, timeout, distance):
|
||||
goalReached = False
|
||||
start_time = time.time()
|
||||
|
||||
while not goalReached:
|
||||
rclpy.spin_once(self, timeout_sec=1)
|
||||
if self.distanceFromGoal() < distance:
|
||||
goalReached = True
|
||||
self.info_msg('*** GOAL REACHED ***')
|
||||
return True
|
||||
elif timeout is not None:
|
||||
if (time.time() - start_time) > timeout:
|
||||
self.error_msg('Robot timed out reaching its goal!')
|
||||
return False
|
||||
|
||||
def distanceFromGoal(self):
|
||||
d_x = self.current_pose.position.x - self.goal_pose.position.x
|
||||
d_y = self.current_pose.position.y - self.goal_pose.position.y
|
||||
distance = math.sqrt(d_x * d_x + d_y * d_y)
|
||||
self.info_msg(f'Distance from goal is: {distance}')
|
||||
return distance
|
||||
|
||||
def wait_for_node_active(self, node_name: str):
|
||||
# Waits for the node within the tester namespace to become active
|
||||
self.info_msg(f'Waiting for {node_name} to become active')
|
||||
node_service = f'{node_name}/get_state'
|
||||
state_client = self.create_client(GetState, node_service)
|
||||
while not state_client.wait_for_service(timeout_sec=1.0):
|
||||
self.info_msg(f'{node_service} service not available, waiting...')
|
||||
req = GetState.Request() # empty request
|
||||
state = 'UNKNOWN'
|
||||
while (state != 'active'):
|
||||
self.info_msg(f'Getting {node_name} state...')
|
||||
future = state_client.call_async(req)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
if future.result() is not None:
|
||||
state = future.result().current_state.label
|
||||
self.info_msg(f'Result of get_state: {state}')
|
||||
else:
|
||||
self.error_msg('Exception while calling service: %r' %
|
||||
future.exception())
|
||||
time.sleep(5)
|
||||
|
||||
def shutdown(self):
|
||||
self.info_msg('Shutting down')
|
||||
self.action_client.destroy()
|
||||
|
||||
transition_service = 'lifecycle_manager_navigation/manage_nodes'
|
||||
mgr_client = self.create_client(
|
||||
ManageLifecycleNodes, transition_service)
|
||||
while not mgr_client.wait_for_service(timeout_sec=1.0):
|
||||
self.info_msg(f'{transition_service} service not available, waiting...')
|
||||
|
||||
req = ManageLifecycleNodes.Request()
|
||||
req.command = ManageLifecycleNodes.Request().SHUTDOWN
|
||||
future = mgr_client.call_async(req)
|
||||
try:
|
||||
self.info_msg('Shutting down navigation lifecycle manager...')
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
future.result()
|
||||
self.info_msg(
|
||||
'Shutting down navigation lifecycle manager complete.')
|
||||
except Exception as e: # noqa: B902
|
||||
self.error_msg(f'Service call failed {e!r}')
|
||||
transition_service = 'lifecycle_manager_localization/manage_nodes'
|
||||
mgr_client = self.create_client(
|
||||
ManageLifecycleNodes, transition_service)
|
||||
while not mgr_client.wait_for_service(timeout_sec=1.0):
|
||||
self.info_msg(f'{transition_service} service not available, waiting...')
|
||||
|
||||
req = ManageLifecycleNodes.Request()
|
||||
req.command = ManageLifecycleNodes.Request().SHUTDOWN
|
||||
future = mgr_client.call_async(req)
|
||||
try:
|
||||
self.info_msg('Shutting down localization lifecycle manager...')
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
future.result()
|
||||
self.info_msg(
|
||||
'Shutting down localization lifecycle manager complete')
|
||||
except Exception as e: # noqa: B902
|
||||
self.error_msg(f'Service call failed {e!r}')
|
||||
|
||||
def wait_for_initial_pose(self):
|
||||
self.initial_pose_received = False
|
||||
while not self.initial_pose_received:
|
||||
self.info_msg('Setting initial pose')
|
||||
self.setInitialPose()
|
||||
self.info_msg('Waiting for amcl_pose to be received')
|
||||
rclpy.spin_once(self, timeout_sec=1)
|
||||
|
||||
|
||||
def test_RobotMovesToGoal(robot_tester):
|
||||
robot_tester.info_msg('Setting goal pose')
|
||||
robot_tester.publishGoalPose()
|
||||
robot_tester.info_msg('Waiting 60 seconds for robot to reach goal')
|
||||
return robot_tester.reachesGoal(timeout=60, distance=0.5)
|
||||
|
||||
|
||||
# Tests that all received speed limits are correct:
|
||||
# If overall "filter_test_result" is true
|
||||
# checks that all items in "limit_passed" permissive array are also true.
|
||||
# In other words, it verifies that all speed limits are received
|
||||
# exactly (by count and values) as expected by "limits" array.
|
||||
def test_SpeedLimitsAllCorrect(robot_tester):
|
||||
if not robot_tester.filter_test_result:
|
||||
return False
|
||||
for passed in robot_tester.limit_passed:
|
||||
if not passed:
|
||||
robot_tester.error_msg('Did not meet one of the speed limit')
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def run_all_tests(robot_tester):
|
||||
# set transforms to use_sim_time
|
||||
result = True
|
||||
if (result):
|
||||
robot_tester.wait_for_node_active('amcl')
|
||||
robot_tester.wait_for_initial_pose()
|
||||
robot_tester.wait_for_node_active('bt_navigator')
|
||||
result = robot_tester.wait_for_filter_mask(10)
|
||||
if (result):
|
||||
result = robot_tester.runNavigateAction()
|
||||
|
||||
if robot_tester.test_type == TestType.KEEPOUT:
|
||||
result = result and robot_tester.wait_for_pointcloud_subscribers(10)
|
||||
|
||||
if (result):
|
||||
result = test_RobotMovesToGoal(robot_tester)
|
||||
|
||||
if (result):
|
||||
if robot_tester.test_type == TestType.KEEPOUT:
|
||||
result = robot_tester.filter_test_result
|
||||
result = result and robot_tester.cost_cloud_received
|
||||
elif robot_tester.test_type == TestType.SPEED:
|
||||
result = test_SpeedLimitsAllCorrect(robot_tester)
|
||||
|
||||
# Add more tests here if desired
|
||||
|
||||
if (result):
|
||||
robot_tester.info_msg('Test PASSED')
|
||||
else:
|
||||
robot_tester.error_msg('Test FAILED')
|
||||
|
||||
return result
|
||||
|
||||
|
||||
def fwd_pose(x=0.0, y=0.0, z=0.01):
|
||||
initial_pose = Pose()
|
||||
initial_pose.position.x = x
|
||||
initial_pose.position.y = y
|
||||
initial_pose.position.z = z
|
||||
initial_pose.orientation.x = 0.0
|
||||
initial_pose.orientation.y = 0.0
|
||||
initial_pose.orientation.z = 0.0
|
||||
initial_pose.orientation.w = 1.0
|
||||
return initial_pose
|
||||
|
||||
|
||||
def get_tester(args):
|
||||
|
||||
# Requested tester for one robot
|
||||
type_str = args.type
|
||||
init_x, init_y, final_x, final_y = args.robot[0]
|
||||
test_type = TestType.KEEPOUT # Default value
|
||||
if type_str == 'speed':
|
||||
test_type = TestType.SPEED
|
||||
|
||||
tester = NavTester(
|
||||
test_type,
|
||||
initial_pose=fwd_pose(float(init_x), float(init_y)),
|
||||
goal_pose=fwd_pose(float(final_x), float(final_y)))
|
||||
tester.info_msg(
|
||||
'Starting tester, robot going from ' + init_x + ', ' + init_y +
|
||||
' to ' + final_x + ', ' + final_y + '.')
|
||||
return tester
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
# The robot(s) positions from the input arguments
|
||||
parser = argparse.ArgumentParser(
|
||||
description='System-level costmap filters tester node')
|
||||
parser.add_argument('-t', '--type', type=str, action='store', dest='type',
|
||||
help='Type of costmap filter being tested.')
|
||||
group = parser.add_mutually_exclusive_group(required=True)
|
||||
group.add_argument('-r', '--robot', action='append', nargs=4,
|
||||
metavar=('init_x', 'init_y', 'final_x', 'final_y'),
|
||||
help='The robot starting and final positions.')
|
||||
|
||||
args, unknown = parser.parse_known_args()
|
||||
|
||||
rclpy.init()
|
||||
|
||||
# Create tester for the robot
|
||||
tester = get_tester(args)
|
||||
|
||||
# wait a few seconds to make sure entire stacks are up
|
||||
time.sleep(10)
|
||||
|
||||
passed = run_all_tests(tester)
|
||||
|
||||
# stop and shutdown the nav stack to exit cleanly
|
||||
tester.shutdown()
|
||||
tester.info_msg('Done Shutting Down.')
|
||||
|
||||
if not passed:
|
||||
tester.info_msg('Exiting failed')
|
||||
exit(1)
|
||||
else:
|
||||
tester.info_msg('Exiting passed')
|
||||
exit(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,13 @@
|
||||
add_executable(dummy_controller_node
|
||||
src/dummy_controller/main.cpp
|
||||
src/dummy_controller/dummy_controller.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(dummy_controller_node
|
||||
rclcpp
|
||||
std_msgs
|
||||
nav2_util
|
||||
nav2_behavior_tree
|
||||
nav2_msgs
|
||||
nav_msgs
|
||||
)
|
||||
@@ -0,0 +1,104 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <chrono>
|
||||
#include <ctime>
|
||||
#include <thread>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
|
||||
#include "dummy_controller.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
DummyController::DummyController()
|
||||
: Node("DummyController")
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Initializing DummyController...");
|
||||
|
||||
auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](auto) {});
|
||||
|
||||
vel_pub_ =
|
||||
this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
|
||||
|
||||
task_server_ = std::make_unique<nav2_behavior_tree::FollowPathTaskServer>(temp_node, false),
|
||||
task_server_->setExecuteCallback(
|
||||
std::bind(&DummyController::followPath, this, std::placeholders::_1));
|
||||
|
||||
// Start listening for incoming ComputePathToPose action server requests
|
||||
task_server_->start();
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Initialized DummyController");
|
||||
}
|
||||
|
||||
DummyController::~DummyController()
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Shutting down DummyController");
|
||||
}
|
||||
|
||||
void
|
||||
DummyController::followPath(const nav2_behavior_tree::FollowPathCommand::SharedPtr /*command*/)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Starting controller ");
|
||||
|
||||
auto start_time = std::chrono::system_clock::now();
|
||||
auto time_since_msg = std::chrono::system_clock::now();
|
||||
|
||||
while (true) {
|
||||
// Dummy controller computation time
|
||||
std::this_thread::sleep_for(50ms);
|
||||
|
||||
if (task_server_->cancelRequested()) {
|
||||
RCLCPP_INFO(get_logger(), "Task cancelled");
|
||||
setZeroVelocity();
|
||||
task_server_->setCanceled();
|
||||
return;
|
||||
}
|
||||
|
||||
// Log a message every second
|
||||
auto current_time = std::chrono::system_clock::now();
|
||||
if (current_time - time_since_msg >= 1s) {
|
||||
RCLCPP_INFO(get_logger(), "Following path");
|
||||
time_since_msg = std::chrono::system_clock::now();
|
||||
}
|
||||
|
||||
// Output control command
|
||||
auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
|
||||
cmd_vel->linear.x = 0.1;
|
||||
vel_pub_->publish(std::move(cmd_vel));
|
||||
|
||||
if (current_time - start_time >= 30s) {
|
||||
RCLCPP_INFO(get_logger(), "Reached end point");
|
||||
setZeroVelocity();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
nav2_behavior_tree::FollowPathResult result;
|
||||
task_server_->setResult(result);
|
||||
}
|
||||
|
||||
void DummyController::setZeroVelocity()
|
||||
{
|
||||
auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
|
||||
cmd_vel->linear.x = 0.0;
|
||||
cmd_vel->linear.y = 0.0;
|
||||
cmd_vel->angular.z = 0.0;
|
||||
vel_pub_->publish(std::move(cmd_vel));
|
||||
}
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
@@ -0,0 +1,45 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_
|
||||
#define DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_behavior_tree/follow_path_task.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
class DummyController : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
DummyController();
|
||||
~DummyController();
|
||||
|
||||
nav2_behavior_tree::TaskStatus followPath(
|
||||
const nav2_behavior_tree::FollowPathCommand::SharedPtr command);
|
||||
|
||||
private:
|
||||
void setZeroVelocity();
|
||||
|
||||
std::unique_ptr<nav2_behavior_tree::FollowPathTaskServer> task_server_;
|
||||
|
||||
std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::Twist>> vel_pub_;
|
||||
};
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
|
||||
#endif // DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_
|
||||
@@ -0,0 +1,26 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <memory>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dummy_controller.hpp"
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<nav2_system_tests::DummyController>());
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
add_executable(dummy_planner_node
|
||||
src/dummy_planner/main.cpp
|
||||
src/dummy_planner/dummy_planner.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(dummy_planner_node
|
||||
rclcpp
|
||||
std_msgs
|
||||
nav2_util
|
||||
nav2_behavior_tree
|
||||
nav2_msgs
|
||||
nav_msgs
|
||||
)
|
||||
@@ -0,0 +1,73 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <memory>
|
||||
|
||||
#include "dummy_planner.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
DummyPlanner::DummyPlanner()
|
||||
: Node("DummyPlanner")
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Initializing DummyPlanner...");
|
||||
|
||||
auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](auto) {});
|
||||
|
||||
task_server_ =
|
||||
std::make_unique<nav2_behavior_tree::ComputePathToPoseTaskServer>(temp_node, false),
|
||||
task_server_->setExecuteCallback(
|
||||
std::bind(&DummyPlanner::computePlan, this, std::placeholders::_1));
|
||||
|
||||
// Start listening for incoming ComputePathToPose task requests
|
||||
task_server_->start();
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Initialized DummyPlanner");
|
||||
}
|
||||
|
||||
DummyPlanner::~DummyPlanner()
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Shutting down DummyPlanner");
|
||||
}
|
||||
|
||||
void
|
||||
DummyPlanner::computePlan(const nav2_behavior_tree::ComputePathToPoseCommand::SharedPtr cmd)
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
get_logger(), "Attempting to a find path from (%.2f, %.2f) to "
|
||||
"(%.2f, %.2f).", cmd->start.position.x, cmd->start.position.y,
|
||||
cmd->goal.position.x, cmd->goal.position.y);
|
||||
|
||||
// Dummy path computation time
|
||||
std::this_thread::sleep_for(500ms);
|
||||
|
||||
if (task_server_->cancelRequested()) {
|
||||
RCLCPP_INFO(get_logger(), "Cancelled planning task.");
|
||||
task_server_->setCanceled();
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Found a dummy path");
|
||||
|
||||
nav2_behavior_tree::ComputePathToPoseResult result;
|
||||
// set succeeded
|
||||
task_server_->setResult(result);
|
||||
}
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
@@ -0,0 +1,40 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef DUMMY_PLANNER__DUMMY_PLANNER_HPP_
|
||||
#define DUMMY_PLANNER__DUMMY_PLANNER_HPP_
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_behavior_tree/compute_path_to_pose_task.hpp"
|
||||
|
||||
namespace nav2_system_tests
|
||||
{
|
||||
|
||||
class DummyPlanner : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
DummyPlanner();
|
||||
~DummyPlanner();
|
||||
|
||||
nav2_behavior_tree::TaskStatus computePathToPose(
|
||||
const nav2_behavior_tree::ComputePathToPoseCommand::SharedPtr command);
|
||||
|
||||
private:
|
||||
std::unique_ptr<nav2_behavior_tree::ComputePathToPoseTaskServer> task_server_;
|
||||
};
|
||||
|
||||
} // namespace nav2_system_tests
|
||||
|
||||
#endif // DUMMY_PLANNER__DUMMY_PLANNER_HPP_
|
||||
@@ -0,0 +1,26 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <memory>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dummy_planner.hpp"
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<nav2_system_tests::DummyPlanner>());
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
ament_add_gtest_executable(test_localization_node
|
||||
test_localization_node.cpp
|
||||
)
|
||||
ament_target_dependencies(test_localization_node
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
|
||||
ament_add_test(test_localization
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_localization_launch.py"
|
||||
TIMEOUT 180
|
||||
ENV
|
||||
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
|
||||
TEST_EXECUTABLE=$<TARGET_FILE:test_localization_node>
|
||||
TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
|
||||
GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
|
||||
)
|
||||
@@ -0,0 +1,22 @@
|
||||
# Localization Testing
|
||||
|
||||
The intention of the localization test is to ensure robot’s pose and transforms are available.
|
||||
|
||||
Currently, only a simple test that checks the `initialpose` has been implemented. The `test_localization` module publishes an initial pose on `initialpose` topic and then it listens to `amcl_pose` topic. If the `amcl_pose` is similar to `initial pose` within a predefined tolerance the test passes.
|
||||
|
||||
## To run the test
|
||||
First, build the package
|
||||
```
|
||||
colcon build --symlink-install
|
||||
```
|
||||
After building, from /build/nav2_system_tests directory run:
|
||||
```
|
||||
ctest -V -R test_localization
|
||||
```
|
||||
Alternately you can run all the tests in the package using colcon:
|
||||
```
|
||||
colcon test --packages-select nav2_system_tests
|
||||
```
|
||||
|
||||
## Future Plan
|
||||
Once rosbag functionality becomes available, this test can be extended to utilize a recorded trajectory with map and scan data to monitor the `amcl_pose` and `transforms` without a need to run Gazebo and map_server.
|
||||
@@ -0,0 +1,79 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# Copyright (c) 2018 Intel Corporation
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import os
|
||||
import sys
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch import LaunchService
|
||||
import launch.actions
|
||||
from launch.actions import ExecuteProcess
|
||||
import launch_ros.actions
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
mapFile = os.getenv('TEST_MAP')
|
||||
testExecutable = os.getenv('TEST_EXECUTABLE')
|
||||
world = os.getenv('TEST_WORLD')
|
||||
|
||||
launch_gazebo = launch.actions.ExecuteProcess(
|
||||
cmd=['gzserver', '-s', 'libgazebo_ros_init.so', '--minimal_comms', world],
|
||||
output='screen')
|
||||
link_footprint = launch_ros.actions.Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'])
|
||||
footprint_scan = launch_ros.actions.Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'])
|
||||
run_map_server = launch_ros.actions.Node(
|
||||
package='nav2_map_server',
|
||||
executable='map_server',
|
||||
name='map_server',
|
||||
output='screen',
|
||||
parameters=[{'yaml_filename': mapFile}])
|
||||
run_amcl = launch_ros.actions.Node(
|
||||
package='nav2_amcl',
|
||||
executable='amcl',
|
||||
output='screen')
|
||||
run_lifecycle_manager = launch_ros.actions.Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager',
|
||||
output='screen',
|
||||
parameters=[{'node_names': ['map_server', 'amcl']}, {'autostart': True}])
|
||||
ld = LaunchDescription([launch_gazebo, link_footprint, footprint_scan,
|
||||
run_map_server, run_amcl, run_lifecycle_manager])
|
||||
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[testExecutable],
|
||||
name='test_localization_node',
|
||||
output='screen'
|
||||
)
|
||||
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,122 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <memory>
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_amcl/amcl_node.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
#include "geometry_msgs/msg/pose_array.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
|
||||
using std::placeholders::_1;
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// rclcpp::init can only be called once per process, so this needs to be a global variable
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
class TestAmclPose : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
TestAmclPose()
|
||||
{
|
||||
pose_callback_ = false;
|
||||
initTestPose();
|
||||
tol_ = 0.25;
|
||||
|
||||
node = rclcpp::Node::make_shared("localization_test");
|
||||
|
||||
while (node->count_subscribers("scan") < 1) {
|
||||
std::this_thread::sleep_for(100ms);
|
||||
rclcpp::spin_some(node);
|
||||
}
|
||||
|
||||
initial_pose_pub_ = node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
|
||||
"initialpose", rclcpp::SystemDefaultsQoS());
|
||||
subscription_ = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
|
||||
"amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
|
||||
std::bind(&TestAmclPose::amcl_pose_callback, this, _1));
|
||||
initial_pose_pub_->publish(testPose_);
|
||||
}
|
||||
|
||||
bool defaultAmclTest();
|
||||
|
||||
protected:
|
||||
std::shared_ptr<rclcpp::Node> node;
|
||||
void initTestPose();
|
||||
|
||||
private:
|
||||
void amcl_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
|
||||
{
|
||||
auto amcl_pose = msg->pose;
|
||||
amcl_pose_x = amcl_pose.pose.position.x;
|
||||
amcl_pose_y = amcl_pose.pose.position.y;
|
||||
pose_callback_ = true;
|
||||
}
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped testPose_;
|
||||
double amcl_pose_x;
|
||||
double amcl_pose_y;
|
||||
bool pose_callback_;
|
||||
float tol_;
|
||||
};
|
||||
|
||||
bool TestAmclPose::defaultAmclTest()
|
||||
{
|
||||
initial_pose_pub_->publish(testPose_);
|
||||
while (!pose_callback_) {
|
||||
// TODO(mhpanah): Initial pose should only be published once.
|
||||
initial_pose_pub_->publish(testPose_);
|
||||
std::this_thread::sleep_for(1s);
|
||||
rclcpp::spin_some(node);
|
||||
}
|
||||
if (std::abs(amcl_pose_x - testPose_.pose.pose.position.x) < tol_ &&
|
||||
std::abs(amcl_pose_y - testPose_.pose.pose.position.y) < tol_)
|
||||
{
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void TestAmclPose::initTestPose()
|
||||
{
|
||||
testPose_.header.frame_id = "map";
|
||||
testPose_.header.stamp = rclcpp::Time();
|
||||
testPose_.pose.pose.position.x = -2.0;
|
||||
testPose_.pose.pose.position.y = -0.5;
|
||||
testPose_.pose.pose.position.z = 0.0;
|
||||
testPose_.pose.pose.orientation.x = 0.0;
|
||||
testPose_.pose.pose.orientation.y = 0.0;
|
||||
testPose_.pose.pose.orientation.z = 0.0;
|
||||
testPose_.pose.pose.orientation.w = 1.0;
|
||||
for (int i = 0; i < 35; i++) {
|
||||
testPose_.pose.covariance[i] = 0.0;
|
||||
}
|
||||
testPose_.pose.covariance[0] = 0.08;
|
||||
testPose_.pose.covariance[7] = 0.08;
|
||||
testPose_.pose.covariance[35] = 0.05;
|
||||
}
|
||||
|
||||
TEST_F(TestAmclPose, SimpleAmclTest)
|
||||
{
|
||||
EXPECT_EQ(true, defaultAmclTest());
|
||||
}
|
||||
@@ -0,0 +1,63 @@
|
||||
set(test_planner_costmaps_exec test_planner_costmaps_node)
|
||||
|
||||
ament_add_gtest_executable(${test_planner_costmaps_exec}
|
||||
test_planner_costmaps_node.cpp
|
||||
planner_tester.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(${test_planner_costmaps_exec}
|
||||
${nav2_map_server_LIBRARIES})
|
||||
|
||||
ament_target_dependencies(${test_planner_costmaps_exec}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
set(test_planner_random_exec test_planner_random_node)
|
||||
|
||||
ament_add_gtest_executable(${test_planner_random_exec}
|
||||
test_planner_random_node.cpp
|
||||
planner_tester.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${test_planner_random_exec}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
target_link_libraries(${test_planner_random_exec}
|
||||
${nav2_map_server_LIBRARIES})
|
||||
|
||||
ament_add_test(test_planner_costmaps
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_planner_costmaps_launch.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
ENV
|
||||
TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
|
||||
TEST_EXECUTABLE=$<TARGET_FILE:${test_planner_costmaps_exec}>
|
||||
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm
|
||||
)
|
||||
|
||||
ament_add_test(test_planner_random
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_planner_random_launch.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
ENV
|
||||
TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
|
||||
TEST_EXECUTABLE=$<TARGET_FILE:${test_planner_random_exec}>
|
||||
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm
|
||||
)
|
||||
|
||||
ament_add_gtest(test_planner_plugins
|
||||
test_planner_plugins.cpp
|
||||
TIMEOUT 10
|
||||
)
|
||||
|
||||
ament_target_dependencies(test_planner_plugins rclcpp geometry_msgs nav2_msgs ${dependencies})
|
||||
target_link_libraries(test_planner_plugins
|
||||
# stdc++fs
|
||||
)
|
||||
|
||||
ament_add_gtest(test_planner_is_path_valid
|
||||
test_planner_is_path_valid.cpp
|
||||
planner_tester.cpp)
|
||||
|
||||
ament_target_dependencies(test_planner_is_path_valid rclcpp geometry_msgs nav2_msgs ${dependencies})
|
||||
@@ -0,0 +1,15 @@
|
||||
# Global Planner Component Testing
|
||||
|
||||
A PlannerTester node provides the world representation in the form of a costmap, sends a request to generate a path, and receives and checks the quality of the generated path.
|
||||
|
||||
As mentioned above, currently the world is represented as a costmap. Simplified versions of the world model and costmap are used for testing.
|
||||
|
||||
PlannerTester can sequentially pass random starting and goal poses and check the returned path for possible collision along the path.
|
||||
|
||||
Below is an example of the output from randomized testing. Blue spheres represent the starting locations, green, the goals. Red lines are the computed paths. Grey cells represent obstacles.
|
||||
|
||||
|
||||
|
||||
*Note: Currently robot size is 1x1 cells, no obstacle inflation is done on the costmap*
|
||||
|
||||
*Note: The Navfn algorithm sometimes fails to generate a path as you can see from the 'orphan' spheres.*
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 285 KiB |
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user