Skip to content
Snippets Groups Projects
Unverified Commit 87188c66 authored by Steven Macenski's avatar Steven Macenski Committed by GitHub
Browse files

Adding failure to navigate test (#1912)

* failures tests

* adding copyrights

* cancel test in recoveries
parent 8501a84d
No related branches found
No related tags found
No related merge requests found
Showing
with 522 additions and 19 deletions
......@@ -312,9 +312,6 @@ BtNavigator::navigateToPose()
RCLCPP_INFO(get_logger(), "Navigation canceled");
action_server_->terminate_all();
break;
default:
throw std::logic_error("Invalid status return from BT");
}
}
......
......@@ -278,13 +278,8 @@ void ControllerServer::computeControl()
rclcpp::Rate loop_rate(controller_frequency_);
while (rclcpp::ok()) {
if (action_server_ == nullptr) {
RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping.");
return;
}
if (!action_server_->is_server_active()) {
RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping.");
if (action_server_ == nullptr || !action_server_->is_server_active()) {
RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping.");
return;
}
......
......@@ -211,13 +211,8 @@ PlannerServer::computePlan()
auto result = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>();
try {
if (action_server_ == nullptr) {
RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping.");
return;
}
if (!action_server_->is_server_active()) {
RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping.");
if (action_server_ == nullptr || !action_server_->is_server_active()) {
RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping.");
return;
}
......
......@@ -55,6 +55,7 @@ if(BUILD_TESTING)
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/recoveries/spin)
......
......@@ -71,6 +71,8 @@ void PlannerTester::activate()
"GridBased.use_astar", rclcpp::ParameterValue(true));
planner_tester_->set_parameter(
rclcpp::Parameter(std::string("GridBased.use_astar"), rclcpp::ParameterValue(true)));
planner_tester_->set_parameter(
rclcpp::Parameter(std::string("expected_planner_frequency"), rclcpp::ParameterValue(-1.0)));
planner_tester_->onConfigure(state);
publishRobotTransform();
map_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>("map", 1);
......
......@@ -62,11 +62,16 @@ WaitRecoveryTester * WaitRecoveryTestFixture::wait_recovery_tester = nullptr;
TEST_P(WaitRecoveryTestFixture, testSWaitRecovery)
{
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++) {
success = success || wait_recovery_tester->recoveryTest(wait_time);
if (cancel == 1.0) {
success = success || wait_recovery_tester->recoveryTestCancel(wait_time);
} else {
success = success || wait_recovery_tester->recoveryTest(wait_time);
}
if (success) {
break;
}
......@@ -81,7 +86,8 @@ INSTANTIATE_TEST_CASE_P(
::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(5.0, 0.0),
std::make_tuple(10.0, 1.0)),
testNameGenerator);
int main(int argc, char ** argv)
......
......@@ -172,6 +172,83 @@ bool WaitRecoveryTester::recoveryTest(
return true;
}
bool WaitRecoveryTester::recoveryTestCancel(
const float wait_time)
{
if (!is_active_) {
RCLCPP_ERROR(node_->get_logger(), "Not activated");
return false;
}
// Sleep to let recovery 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 WaitRecoveryTester::sendInitialPose()
{
geometry_msgs::msg::PoseWithCovarianceStamped pose;
......
......@@ -52,6 +52,8 @@ public:
bool recoveryTest(
float time);
bool recoveryTestCancel(float time);
void activate();
void deactivate();
......
ament_add_test(test_failure_navigator
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_failure_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_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml
)
# Navigation2 System Tests - Failure
High level system failures tests
#!/usr/bin/env python3
# Copyright (c) 2018 Intel Corporation
# Copyright (c) 2020 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
param_substitutions = {
'planner_server.ros__parameters.GridBased.use_astar': 'False'}
configured_params = RewrittenYaml(
source_file=params_file,
root_key='',
param_rewrites=param_substitutions,
convert_types=True)
return LaunchDescription([
SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '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={'namespace': '',
'use_namespace': 'False',
'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()
test1_action = ExecuteProcess(
cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'),
'-r', '-2.0', '-0.5', '100.0', '100.0'],
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())
#! /usr/bin/env python3
# Copyright 2018 Intel Corporation.
# Copyright 2020 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 argparse
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.srv import ManageLifecycleNodes
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
from rclpy.qos import QoSProfile
class NavTester(Node):
def __init__(
self,
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)
pose_qos = QoSProfile(
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1)
self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
'amcl_pose', self.poseCallback, pose_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_ABORTED:
self.info_msg('Goal failed with status code: {0}'.format(status))
return False
self.info_msg('Goal failed, as expected!')
return True
def poseCallback(self, msg):
self.info_msg('Received amcl_pose')
self.current_pose = msg.pose.pose
self.initial_pose_received = 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('Distance from goal is: ' + str(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('Waiting for ' + node_name + ' to become active')
node_service = 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(node_service + ' service not available, waiting...')
req = GetState.Request() # empty request
state = 'UNKNOWN'
while (state != 'active'):
self.info_msg('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('Result of get_state: %s' % 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(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:
self.error_msg('Service call failed %r' % (e,))
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(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:
self.error_msg('Service call failed %r' % (e,))
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 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.runNavigateAction()
# 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_testers(args):
testers = []
if args.robot:
# Requested tester for one robot
init_x, init_y, final_x, final_y = args.robot[0]
tester = NavTester(
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 + '.')
testers.append(tester)
return testers
return testers
def main(argv=sys.argv[1:]):
# The robot(s) positions from the input arguments
parser = argparse.ArgumentParser(description='System-level navigation tester node')
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.')
group.add_argument('-rs', '--robots', action='append', nargs=5,
metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'),
help="The robot's namespace and starting and final positions. " +
'Repeating the argument for multiple robots is supported.')
args, unknown = parser.parse_known_args()
rclpy.init()
# Create testers for each robot
testers = get_testers(args)
# wait a few seconds to make sure entire stacks are up
time.sleep(10)
for tester in testers:
passed = run_all_tests(tester)
if not passed:
break
for tester in testers:
# stop and shutdown the nav stack to exit cleanly
tester.shutdown()
testers[0].info_msg('Done Shutting Down.')
if not passed:
testers[0].info_msg('Exiting failed')
exit(1)
else:
testers[0].info_msg('Exiting passed')
exit(0)
if __name__ == '__main__':
main()
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment