add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,178 @@
|
||||
// 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. Reserved.
|
||||
|
||||
#include <math.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <limits>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_subscriber.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_smac_planner/node_hybrid.hpp"
|
||||
#include "nav2_smac_planner/a_star.hpp"
|
||||
#include "nav2_smac_planner/collision_checker.hpp"
|
||||
#include "nav2_smac_planner/smoother.hpp"
|
||||
#include "ament_index_cpp/get_package_share_directory.hpp"
|
||||
|
||||
using namespace nav2_smac_planner; // NOLINT
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
class SmootherWrapper : public nav2_smac_planner::Smoother
|
||||
{
|
||||
public:
|
||||
explicit SmootherWrapper(const SmootherParams & params)
|
||||
: nav2_smac_planner::Smoother(params)
|
||||
{}
|
||||
|
||||
std::vector<PathSegment> findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path)
|
||||
{
|
||||
return findDirectionalPathSegments(path);
|
||||
}
|
||||
};
|
||||
|
||||
TEST(SmootherTest, test_full_smoother)
|
||||
{
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr node =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSmootherTest");
|
||||
nav2_smac_planner::SmootherParams params;
|
||||
params.get(node, "test");
|
||||
double maxtime = 1.0;
|
||||
|
||||
// Make smoother and costmap to smooth in
|
||||
auto smoother = std::make_unique<SmootherWrapper>(params);
|
||||
smoother->initialize(0.4 /*turning radius*/);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmap =
|
||||
new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0.0, 0.0, 0);
|
||||
// island in the middle of lethal cost to cross
|
||||
for (unsigned int i = 20; i <= 30; ++i) {
|
||||
for (unsigned int j = 20; j <= 30; ++j) {
|
||||
costmap->setCost(i, j, 254);
|
||||
}
|
||||
}
|
||||
|
||||
// Setup A* search to get path to smooth
|
||||
nav2_smac_planner::SearchInfo info;
|
||||
info.change_penalty = 0.05;
|
||||
info.non_straight_penalty = 1.05;
|
||||
info.reverse_penalty = 2.0;
|
||||
info.cost_penalty = 2.0;
|
||||
info.retrospective_penalty = 0.0;
|
||||
info.analytic_expansion_ratio = 3.5;
|
||||
info.minimum_turning_radius = 8; // in grid coordinates 0.4/0.05
|
||||
info.analytic_expansion_max_length = 20.0; // in grid coordinates
|
||||
unsigned int size_theta = 72;
|
||||
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
|
||||
nav2_smac_planner::MotionModel::REEDS_SHEPP, info);
|
||||
int max_iterations = 10000;
|
||||
float tolerance = 10.0;
|
||||
int it_on_approach = 10;
|
||||
double max_planning_time = 120.0;
|
||||
int num_it = 0;
|
||||
|
||||
a_star.initialize(
|
||||
false, max_iterations, std::numeric_limits<int>::max(), max_planning_time, 401, size_theta);
|
||||
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
|
||||
std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap, size_theta, node);
|
||||
checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
|
||||
|
||||
// Create A* search to smooth
|
||||
a_star.setCollisionChecker(checker.get());
|
||||
a_star.setStart(5u, 5u, 0u);
|
||||
a_star.setGoal(45u, 45u, 36u);
|
||||
nav2_smac_planner::NodeHybrid::CoordinateVector path;
|
||||
EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
|
||||
|
||||
// Convert to world coordinates and get length to compare to smoothed length
|
||||
nav_msgs::msg::Path plan;
|
||||
plan.header.stamp = node->now();
|
||||
plan.header.frame_id = "map";
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header = plan.header;
|
||||
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;
|
||||
double initial_length = 0.0;
|
||||
double x_m = path[path.size() - 1].x, y_m = path[path.size() - 1].y;
|
||||
plan.poses.reserve(path.size());
|
||||
for (int i = path.size() - 1; i >= 0; --i) {
|
||||
pose.pose = nav2_smac_planner::getWorldCoords(path[i].x, path[i].y, costmap);
|
||||
pose.pose.orientation = nav2_smac_planner::getWorldOrientation(path[i].theta);
|
||||
plan.poses.push_back(pose);
|
||||
initial_length += hypot(path[i].x - x_m, path[i].y - y_m);
|
||||
x_m = path[i].x;
|
||||
y_m = path[i].y;
|
||||
}
|
||||
|
||||
// Check that we accurately detect that this path has a reversing segment
|
||||
EXPECT_EQ(smoother->findDirectionalPathSegmentsWrapper(plan).size(), 2u);
|
||||
|
||||
// Test smoother, should succeed with same number of points
|
||||
// and shorter overall length, while still being collision free.
|
||||
auto path_size_in = plan.poses.size();
|
||||
EXPECT_TRUE(smoother->smooth(plan, costmap, maxtime));
|
||||
EXPECT_EQ(plan.poses.size(), path_size_in); // Should have same number of poses
|
||||
double length = 0.0;
|
||||
x_m = plan.poses[0].pose.position.x;
|
||||
y_m = plan.poses[0].pose.position.y;
|
||||
for (unsigned int i = 0; i != plan.poses.size(); i++) {
|
||||
// Should be collision free
|
||||
EXPECT_EQ(costmap->getCost(plan.poses[i].pose.position.x, plan.poses[i].pose.position.y), 0);
|
||||
length += hypot(plan.poses[i].pose.position.x - x_m, plan.poses[i].pose.position.y - y_m);
|
||||
x_m = plan.poses[i].pose.position.x;
|
||||
y_m = plan.poses[i].pose.position.y;
|
||||
}
|
||||
EXPECT_LT(length, initial_length); // Should be shorter
|
||||
|
||||
// Try again but with failure modes
|
||||
|
||||
// Failure mode: not enough iterations to complete
|
||||
params.max_its_ = 0;
|
||||
auto smoother_bypass = std::make_unique<SmootherWrapper>(params);
|
||||
EXPECT_FALSE(smoother_bypass->smooth(plan, costmap, maxtime));
|
||||
params.max_its_ = 1;
|
||||
auto smoother_failure = std::make_unique<SmootherWrapper>(params);
|
||||
EXPECT_FALSE(smoother_failure->smooth(plan, costmap, maxtime));
|
||||
|
||||
// Failure mode: Not enough time
|
||||
double max_no_time = 0.0;
|
||||
EXPECT_FALSE(smoother->smooth(plan, costmap, max_no_time));
|
||||
|
||||
// Failure mode: Path is in collision, do 2x to exercise overlapping point
|
||||
// attempts to update orientation should also fail
|
||||
pose.pose.position.x = 1.25;
|
||||
pose.pose.position.y = 1.25;
|
||||
plan.poses.push_back(pose);
|
||||
plan.poses.push_back(pose);
|
||||
EXPECT_FALSE(smoother->smooth(plan, costmap, maxtime));
|
||||
EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.z, 1.0, 1e-3);
|
||||
EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.x, 0.0, 1e-3);
|
||||
EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.y, 0.0, 1e-3);
|
||||
EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.w, 0.0, 1e-3);
|
||||
|
||||
delete costmap;
|
||||
}
|
||||
Reference in New Issue
Block a user