-
Notifications
You must be signed in to change notification settings - Fork 1.3k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Fix infinite rotation in Spin recovery when angle > PI #1670
Merged
SteveMacenski
merged 5 commits into
ros-navigation:master
from
naiveHobo:fix_recovery_spin
May 5, 2020
Merged
Changes from all commits
Commits
Show all changes
5 commits
Select commit
Hold shift + click to select a range
File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,23 @@ | ||
set(test_spin_recovery_exec test_spin_recovery_node) | ||
|
||
ament_add_gtest_executable(${test_spin_recovery_exec} | ||
test_spin_recovery_node.cpp | ||
spin_recovery_tester.cpp | ||
) | ||
|
||
ament_target_dependencies(${test_spin_recovery_exec} | ||
${dependencies} | ||
) | ||
|
||
ament_add_test(test_spin_recovery | ||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO | ||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_recovery_launch.py" | ||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" | ||
TIMEOUT 100 | ||
ENV | ||
TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml | ||
TEST_EXECUTABLE=$<TARGET_FILE:${test_spin_recovery_exec}> | ||
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 | ||
) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,4 @@ | ||
# Recoveries Test | ||
|
||
Provides some simple tests for recovery plugins. | ||
It creates an instance of the stack, with the recovery server loading different recovery plugins, and checks for successful recovery behaviors. |
224 changes: 224 additions & 0 deletions
224
nav2_system_tests/src/recoveries/spin_recovery_tester.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,224 @@ | ||
// Copyright (c) 2020 Sarthak Mittal | ||
// Copyright (c) 2018 Intel Corporation | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. You may want to add your own copyright below this // Copyright (c) 2020 XYZ |
||
// | ||
// 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_recovery_tester.hpp" | ||
|
||
using namespace std::chrono_literals; | ||
using namespace std::chrono; // NOLINT | ||
|
||
namespace nav2_system_tests | ||
{ | ||
|
||
SpinRecoveryTester::SpinRecoveryTester() | ||
: is_active_(false), | ||
initial_pose_received_(false) | ||
{ | ||
node_ = rclcpp::Node::make_shared("spin_recovery_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<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); | ||
|
||
subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>( | ||
"amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), | ||
std::bind(&SpinRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); | ||
} | ||
|
||
SpinRecoveryTester::~SpinRecoveryTester() | ||
{ | ||
if (is_active_) { | ||
deactivate(); | ||
} | ||
} | ||
|
||
void SpinRecoveryTester::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 recoveries_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 SpinRecoveryTester::deactivate() | ||
{ | ||
if (!is_active_) { | ||
throw std::runtime_error("Trying to deactivate while already inactive"); | ||
} | ||
is_active_ = false; | ||
} | ||
|
||
bool SpinRecoveryTester::defaultSpinRecoveryTest( | ||
const float target_yaw, | ||
const double tolerance) | ||
{ | ||
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 goal_msg = Spin::Goal(); | ||
goal_msg.target_yaw = target_yaw; | ||
|
||
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::executor::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"); | ||
if (rclcpp::spin_until_future_complete(node_, result_future) != | ||
rclcpp::executor::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(), | ||
"Angular distance from goal is %lf (tolerance %lf)", | ||
fabs(dyaw), tolerance); | ||
return false; | ||
} | ||
|
||
return true; | ||
} | ||
|
||
void SpinRecoveryTester::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 SpinRecoveryTester::amclPoseCallback( | ||
const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) | ||
{ | ||
initial_pose_received_ = true; | ||
} | ||
|
||
} // namespace nav2_system_tests |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ah, these look awfully modified from source. You should include both intel and yourself.