X-lanni
281 lines
12 KiB
C++
281 lines
12 KiB
C++
// Copyright (c) 2022, 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 <chrono>
|
|
#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_msgs/msg/costmap.hpp"
|
|
#include "nav2_util/lifecycle_node.hpp"
|
|
#include "nav2_smoother/simple_smoother.hpp"
|
|
#include "ament_index_cpp/get_package_share_directory.hpp"
|
|
|
|
using namespace smoother_utils; // NOLINT
|
|
using namespace nav2_smoother; // NOLINT
|
|
using namespace std::chrono_literals; // NOLINT
|
|
|
|
class RclCppFixture
|
|
{
|
|
public:
|
|
RclCppFixture() {rclcpp::init(0, nullptr);}
|
|
~RclCppFixture() {rclcpp::shutdown();}
|
|
};
|
|
RclCppFixture g_rclcppfixture;
|
|
|
|
class SmootherWrapper : public nav2_smoother::SimpleSmoother
|
|
{
|
|
public:
|
|
SmootherWrapper()
|
|
: nav2_smoother::SimpleSmoother()
|
|
{
|
|
}
|
|
|
|
std::vector<PathSegment> findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path)
|
|
{
|
|
return findDirectionalPathSegments(path);
|
|
}
|
|
|
|
void setMaxItsToInvalid()
|
|
{
|
|
max_its_ = 0;
|
|
}
|
|
};
|
|
|
|
TEST(SmootherTest, test_simple_smoother)
|
|
{
|
|
rclcpp_lifecycle::LifecycleNode::SharedPtr node =
|
|
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSmootherTest");
|
|
|
|
std::shared_ptr<nav2_msgs::msg::Costmap> costmap_msg =
|
|
std::make_shared<nav2_msgs::msg::Costmap>();
|
|
costmap_msg->header.stamp = node->now();
|
|
costmap_msg->header.frame_id = "map";
|
|
costmap_msg->data.resize(100 * 100);
|
|
costmap_msg->metadata.resolution = 0.05;
|
|
costmap_msg->metadata.size_x = 100;
|
|
costmap_msg->metadata.size_y = 100;
|
|
|
|
// 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_msg->data[j * 100 + i] = 254;
|
|
}
|
|
}
|
|
|
|
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> parent = node;
|
|
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> dummy_costmap;
|
|
dummy_costmap = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(parent, "dummy_topic");
|
|
dummy_costmap->costmapCallback(costmap_msg);
|
|
|
|
// Make smoother
|
|
std::shared_ptr<tf2_ros::Buffer> dummy_tf;
|
|
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> dummy_footprint;
|
|
auto smoother = std::make_unique<SmootherWrapper>();
|
|
smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint);
|
|
|
|
// Test that an irregular distributed path becomes more distributed
|
|
nav_msgs::msg::Path straight_irregular_path;
|
|
straight_irregular_path.header.frame_id = "map";
|
|
straight_irregular_path.header.stamp = node->now();
|
|
straight_irregular_path.poses.resize(11);
|
|
straight_irregular_path.poses[0].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[0].pose.position.y = 0.0;
|
|
straight_irregular_path.poses[1].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[1].pose.position.y = 0.1;
|
|
straight_irregular_path.poses[2].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[2].pose.position.y = 0.2;
|
|
straight_irregular_path.poses[3].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[3].pose.position.y = 0.35;
|
|
straight_irregular_path.poses[4].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[4].pose.position.y = 0.4;
|
|
straight_irregular_path.poses[5].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[5].pose.position.y = 0.56;
|
|
straight_irregular_path.poses[6].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[6].pose.position.y = 0.9;
|
|
straight_irregular_path.poses[7].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[7].pose.position.y = 0.95;
|
|
straight_irregular_path.poses[8].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[8].pose.position.y = 1.3;
|
|
straight_irregular_path.poses[9].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[9].pose.position.y = 2.0;
|
|
straight_irregular_path.poses[10].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[10].pose.position.y = 2.5;
|
|
|
|
rclcpp::Duration no_time = rclcpp::Duration::from_seconds(0.0); // 0 seconds
|
|
rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1); // 1 second
|
|
EXPECT_FALSE(smoother->smooth(straight_irregular_path, no_time));
|
|
EXPECT_TRUE(smoother->smooth(straight_irregular_path, max_time));
|
|
for (uint i = 0; i != straight_irregular_path.poses.size() - 1; i++) {
|
|
// Check distances are more evenly spaced out now
|
|
EXPECT_LT(
|
|
fabs(
|
|
straight_irregular_path.poses[i].pose.position.y -
|
|
straight_irregular_path.poses[i + 1].pose.position.y), 0.38);
|
|
}
|
|
|
|
// Test regular path, should see no effective change
|
|
nav_msgs::msg::Path straight_regular_path;
|
|
straight_regular_path.header = straight_irregular_path.header;
|
|
straight_regular_path.poses.resize(11);
|
|
straight_regular_path.poses[0].pose.position.x = 0.5;
|
|
straight_regular_path.poses[0].pose.position.y = 0.0;
|
|
straight_regular_path.poses[1].pose.position.x = 0.5;
|
|
straight_regular_path.poses[1].pose.position.y = 0.1;
|
|
straight_regular_path.poses[2].pose.position.x = 0.5;
|
|
straight_regular_path.poses[2].pose.position.y = 0.2;
|
|
straight_regular_path.poses[3].pose.position.x = 0.5;
|
|
straight_regular_path.poses[3].pose.position.y = 0.3;
|
|
straight_regular_path.poses[4].pose.position.x = 0.5;
|
|
straight_regular_path.poses[4].pose.position.y = 0.4;
|
|
straight_regular_path.poses[5].pose.position.x = 0.5;
|
|
straight_regular_path.poses[5].pose.position.y = 0.5;
|
|
straight_regular_path.poses[6].pose.position.x = 0.5;
|
|
straight_regular_path.poses[6].pose.position.y = 0.6;
|
|
straight_regular_path.poses[7].pose.position.x = 0.5;
|
|
straight_regular_path.poses[7].pose.position.y = 0.7;
|
|
straight_regular_path.poses[8].pose.position.x = 0.5;
|
|
straight_regular_path.poses[8].pose.position.y = 0.8;
|
|
straight_regular_path.poses[9].pose.position.x = 0.5;
|
|
straight_regular_path.poses[9].pose.position.y = 0.9;
|
|
straight_regular_path.poses[10].pose.position.x = 0.5;
|
|
straight_regular_path.poses[10].pose.position.y = 1.0;
|
|
|
|
EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time));
|
|
for (uint i = 0; i != straight_regular_path.poses.size() - 1; i++) {
|
|
// Check distances are still very evenly spaced
|
|
EXPECT_NEAR(
|
|
fabs(
|
|
straight_regular_path.poses[i].pose.position.y -
|
|
straight_regular_path.poses[i + 1].pose.position.y), 0.1, 0.001);
|
|
}
|
|
|
|
// test shorter and curved if given a right angle
|
|
nav_msgs::msg::Path right_angle_path;
|
|
right_angle_path = straight_regular_path;
|
|
straight_regular_path.poses[6].pose.position.x = 0.6;
|
|
straight_regular_path.poses[6].pose.position.y = 0.5;
|
|
straight_regular_path.poses[7].pose.position.x = 0.7;
|
|
straight_regular_path.poses[7].pose.position.y = 0.5;
|
|
straight_regular_path.poses[8].pose.position.x = 0.8;
|
|
straight_regular_path.poses[8].pose.position.y = 0.5;
|
|
straight_regular_path.poses[9].pose.position.x = 0.9;
|
|
straight_regular_path.poses[9].pose.position.y = 0.5;
|
|
straight_regular_path.poses[10].pose.position.x = 0.95;
|
|
straight_regular_path.poses[10].pose.position.y = 0.5;
|
|
EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time));
|
|
EXPECT_NEAR(straight_regular_path.poses[5].pose.position.x, 0.637, 0.01);
|
|
EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.353, 0.01);
|
|
|
|
// Test that collisions are rejected
|
|
nav_msgs::msg::Path collision_path;
|
|
collision_path.poses.resize(11);
|
|
collision_path.poses[0].pose.position.x = 0.0;
|
|
collision_path.poses[0].pose.position.y = 0.0;
|
|
collision_path.poses[1].pose.position.x = 0.2;
|
|
collision_path.poses[1].pose.position.y = 0.2;
|
|
collision_path.poses[2].pose.position.x = 0.4;
|
|
collision_path.poses[2].pose.position.y = 0.4;
|
|
collision_path.poses[3].pose.position.x = 0.6;
|
|
collision_path.poses[3].pose.position.y = 0.6;
|
|
collision_path.poses[4].pose.position.x = 0.8;
|
|
collision_path.poses[4].pose.position.y = 0.8;
|
|
collision_path.poses[5].pose.position.x = 1.0;
|
|
collision_path.poses[5].pose.position.y = 1.0;
|
|
collision_path.poses[6].pose.position.x = 1.1;
|
|
collision_path.poses[6].pose.position.y = 1.1;
|
|
collision_path.poses[7].pose.position.x = 1.2;
|
|
collision_path.poses[7].pose.position.y = 1.2;
|
|
collision_path.poses[8].pose.position.x = 1.3;
|
|
collision_path.poses[8].pose.position.y = 1.3;
|
|
collision_path.poses[9].pose.position.x = 1.4;
|
|
collision_path.poses[9].pose.position.y = 1.4;
|
|
collision_path.poses[10].pose.position.x = 1.5;
|
|
collision_path.poses[10].pose.position.y = 1.5;
|
|
EXPECT_FALSE(smoother->smooth(collision_path, max_time));
|
|
|
|
// test cusp / reversing segments
|
|
nav_msgs::msg::Path reversing_path;
|
|
reversing_path.poses.resize(11);
|
|
reversing_path.poses[0].pose.position.x = 0.5;
|
|
reversing_path.poses[0].pose.position.y = 0.0;
|
|
reversing_path.poses[1].pose.position.x = 0.5;
|
|
reversing_path.poses[1].pose.position.y = 0.1;
|
|
reversing_path.poses[2].pose.position.x = 0.5;
|
|
reversing_path.poses[2].pose.position.y = 0.2;
|
|
reversing_path.poses[3].pose.position.x = 0.5;
|
|
reversing_path.poses[3].pose.position.y = 0.3;
|
|
reversing_path.poses[4].pose.position.x = 0.5;
|
|
reversing_path.poses[4].pose.position.y = 0.4;
|
|
reversing_path.poses[5].pose.position.x = 0.5;
|
|
reversing_path.poses[5].pose.position.y = 0.5;
|
|
reversing_path.poses[6].pose.position.x = 0.5;
|
|
reversing_path.poses[6].pose.position.y = 0.4;
|
|
reversing_path.poses[7].pose.position.x = 0.5;
|
|
reversing_path.poses[7].pose.position.y = 0.3;
|
|
reversing_path.poses[8].pose.position.x = 0.5;
|
|
reversing_path.poses[8].pose.position.y = 0.2;
|
|
reversing_path.poses[9].pose.position.x = 0.5;
|
|
reversing_path.poses[9].pose.position.y = 0.1;
|
|
reversing_path.poses[10].pose.position.x = 0.5;
|
|
reversing_path.poses[10].pose.position.y = 0.0;
|
|
EXPECT_TRUE(smoother->smooth(reversing_path, max_time));
|
|
|
|
// // test rotate in place
|
|
tf2::Quaternion quat1, quat2;
|
|
quat1.setRPY(0.0, 0.0, 0.0);
|
|
quat2.setRPY(0.0, 0.0, 1.0);
|
|
straight_irregular_path.poses[5].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[5].pose.position.y = 0.5;
|
|
straight_irregular_path.poses[5].pose.orientation = tf2::toMsg(quat1);
|
|
straight_irregular_path.poses[6].pose.position.x = 0.5;
|
|
straight_irregular_path.poses[6].pose.position.y = 0.5;
|
|
straight_irregular_path.poses[6].pose.orientation = tf2::toMsg(quat2);
|
|
EXPECT_TRUE(smoother->smooth(straight_irregular_path, max_time));
|
|
|
|
// test max iterations
|
|
smoother->setMaxItsToInvalid();
|
|
nav_msgs::msg::Path max_its_path;
|
|
max_its_path.poses.resize(11);
|
|
max_its_path.poses[0].pose.position.x = 0.5;
|
|
max_its_path.poses[0].pose.position.y = 0.0;
|
|
max_its_path.poses[1].pose.position.x = 0.5;
|
|
max_its_path.poses[1].pose.position.y = 0.1;
|
|
max_its_path.poses[2].pose.position.x = 0.5;
|
|
max_its_path.poses[2].pose.position.y = 0.2;
|
|
max_its_path.poses[3].pose.position.x = 0.5;
|
|
max_its_path.poses[3].pose.position.y = 0.3;
|
|
max_its_path.poses[4].pose.position.x = 0.5;
|
|
max_its_path.poses[4].pose.position.y = 0.4;
|
|
max_its_path.poses[5].pose.position.x = 0.5;
|
|
max_its_path.poses[5].pose.position.y = 0.5;
|
|
max_its_path.poses[6].pose.position.x = 0.5;
|
|
max_its_path.poses[6].pose.position.y = 0.6;
|
|
max_its_path.poses[7].pose.position.x = 0.5;
|
|
max_its_path.poses[7].pose.position.y = 0.7;
|
|
max_its_path.poses[8].pose.position.x = 0.5;
|
|
max_its_path.poses[8].pose.position.y = 0.8;
|
|
max_its_path.poses[9].pose.position.x = 0.5;
|
|
max_its_path.poses[9].pose.position.y = 0.9;
|
|
max_its_path.poses[10].pose.position.x = 0.5;
|
|
max_its_path.poses[10].pose.position.y = 1.0;
|
|
EXPECT_FALSE(smoother->smooth(max_its_path, max_time));
|
|
}
|