add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,36 @@
|
||||
ament_add_gtest(test_smoother_server
|
||||
test_smoother_server.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(test_smoother_server
|
||||
${library_name}
|
||||
)
|
||||
|
||||
ament_target_dependencies(test_smoother_server
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
ament_add_gtest(test_simple_smoother
|
||||
test_simple_smoother.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(test_simple_smoother
|
||||
simple_smoother
|
||||
)
|
||||
|
||||
ament_target_dependencies(test_simple_smoother
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
|
||||
ament_add_gtest(test_savitzky_golay_smoother
|
||||
test_savitzky_golay_smoother.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(test_savitzky_golay_smoother
|
||||
savitzky_golay_smoother
|
||||
)
|
||||
|
||||
ament_target_dependencies(test_savitzky_golay_smoother
|
||||
${dependencies}
|
||||
)
|
||||
@@ -0,0 +1,331 @@
|
||||
// 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 <random>
|
||||
|
||||
#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/savitzky_golay_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;
|
||||
|
||||
TEST(SmootherTest, test_sg_smoother_basics)
|
||||
{
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr node =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSGSmootherTest");
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false));
|
||||
auto smoother = std::make_unique<nav2_smoother::SavitzkyGolaySmoother>();
|
||||
smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint);
|
||||
smoother->activate();
|
||||
rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds
|
||||
|
||||
// Test regular path, should see no effective change
|
||||
nav_msgs::msg::Path straight_regular_path, straight_regular_path_baseline;
|
||||
straight_regular_path.header.frame_id = "map";
|
||||
straight_regular_path.header.stamp = node->now();
|
||||
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.1;
|
||||
straight_regular_path.poses[1].pose.position.x = 0.5;
|
||||
straight_regular_path.poses[1].pose.position.y = 0.2;
|
||||
straight_regular_path.poses[2].pose.position.x = 0.5;
|
||||
straight_regular_path.poses[2].pose.position.y = 0.3;
|
||||
straight_regular_path.poses[3].pose.position.x = 0.5;
|
||||
straight_regular_path.poses[3].pose.position.y = 0.4;
|
||||
straight_regular_path.poses[4].pose.position.x = 0.5;
|
||||
straight_regular_path.poses[4].pose.position.y = 0.5;
|
||||
straight_regular_path.poses[5].pose.position.x = 0.5;
|
||||
straight_regular_path.poses[5].pose.position.y = 0.6;
|
||||
straight_regular_path.poses[6].pose.position.x = 0.5;
|
||||
straight_regular_path.poses[6].pose.position.y = 0.7;
|
||||
straight_regular_path.poses[7].pose.position.x = 0.5;
|
||||
straight_regular_path.poses[7].pose.position.y = 0.8;
|
||||
straight_regular_path.poses[8].pose.position.x = 0.5;
|
||||
straight_regular_path.poses[8].pose.position.y = 0.9;
|
||||
straight_regular_path.poses[9].pose.position.x = 0.5;
|
||||
straight_regular_path.poses[9].pose.position.y = 1.0;
|
||||
straight_regular_path.poses[10].pose.position.x = 0.5;
|
||||
straight_regular_path.poses[10].pose.position.y = 1.1;
|
||||
straight_regular_path_baseline = straight_regular_path;
|
||||
|
||||
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 the same
|
||||
EXPECT_NEAR(
|
||||
fabs(
|
||||
straight_regular_path.poses[i].pose.position.y -
|
||||
straight_regular_path_baseline.poses[i].pose.position.y), 0.0, 0.011);
|
||||
}
|
||||
|
||||
// Attempt smoothing with no time given, should fail
|
||||
rclcpp::Duration no_time = rclcpp::Duration::from_seconds(-1.0); // 0 seconds
|
||||
EXPECT_FALSE(smoother->smooth(straight_regular_path, no_time));
|
||||
|
||||
smoother->deactivate();
|
||||
smoother->cleanup();
|
||||
}
|
||||
|
||||
TEST(SmootherTest, test_sg_smoother_noisey_path)
|
||||
{
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr node =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSGSmootherTest");
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false));
|
||||
auto smoother = std::make_unique<nav2_smoother::SavitzkyGolaySmoother>();
|
||||
smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint);
|
||||
rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds
|
||||
|
||||
// Given nominal irregular/noisey path, test that the output is shorter and smoother
|
||||
nav_msgs::msg::Path noisey_path, noisey_path_baseline;
|
||||
noisey_path.header.frame_id = "map";
|
||||
noisey_path.header.stamp = node->now();
|
||||
noisey_path.poses.resize(11);
|
||||
noisey_path.poses[0].pose.position.x = 0.5;
|
||||
noisey_path.poses[0].pose.position.y = 0.1;
|
||||
noisey_path.poses[1].pose.position.x = 0.5;
|
||||
noisey_path.poses[1].pose.position.y = 0.2;
|
||||
noisey_path.poses[2].pose.position.x = 0.5;
|
||||
noisey_path.poses[2].pose.position.y = 0.3;
|
||||
noisey_path.poses[3].pose.position.x = 0.5;
|
||||
noisey_path.poses[3].pose.position.y = 0.4;
|
||||
noisey_path.poses[4].pose.position.x = 0.5;
|
||||
noisey_path.poses[4].pose.position.y = 0.5;
|
||||
noisey_path.poses[5].pose.position.x = 0.5;
|
||||
noisey_path.poses[5].pose.position.y = 0.6;
|
||||
noisey_path.poses[6].pose.position.x = 0.5;
|
||||
noisey_path.poses[6].pose.position.y = 0.7;
|
||||
noisey_path.poses[7].pose.position.x = 0.5;
|
||||
noisey_path.poses[7].pose.position.y = 0.8;
|
||||
noisey_path.poses[8].pose.position.x = 0.5;
|
||||
noisey_path.poses[8].pose.position.y = 0.9;
|
||||
noisey_path.poses[9].pose.position.x = 0.5;
|
||||
noisey_path.poses[9].pose.position.y = 1.0;
|
||||
noisey_path.poses[10].pose.position.x = 0.5;
|
||||
noisey_path.poses[10].pose.position.y = 1.1;
|
||||
|
||||
// Add random but deterministic noises
|
||||
std::random_device rd{};
|
||||
std::mt19937 gen{rd()};
|
||||
std::normal_distribution<> normal_distribution{0.0, 0.02};
|
||||
for (unsigned int i = 0; i != noisey_path.poses.size(); i++) {
|
||||
auto noise = normal_distribution(gen);
|
||||
noisey_path.poses[i].pose.position.x += noise;
|
||||
}
|
||||
|
||||
noisey_path_baseline = noisey_path;
|
||||
EXPECT_TRUE(smoother->smooth(noisey_path, max_time));
|
||||
|
||||
// Compute metric, should be shorter if smoother
|
||||
double length = 0;
|
||||
double base_length = 0;
|
||||
for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) {
|
||||
length += std::hypot(
|
||||
noisey_path.poses[i + 1].pose.position.x - noisey_path.poses[i].pose.position.x,
|
||||
noisey_path.poses[i + 1].pose.position.y - noisey_path.poses[i].pose.position.y);
|
||||
base_length += std::hypot(
|
||||
noisey_path_baseline.poses[i + 1].pose.position.x -
|
||||
noisey_path_baseline.poses[i].pose.position.x,
|
||||
noisey_path_baseline.poses[i + 1].pose.position.y -
|
||||
noisey_path_baseline.poses[i].pose.position.y);
|
||||
}
|
||||
|
||||
EXPECT_LT(length, base_length);
|
||||
|
||||
// Test again with refinement, even shorter and smoother
|
||||
node->set_parameter(rclcpp::Parameter("test.do_refinement", rclcpp::ParameterValue(true)));
|
||||
smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint);
|
||||
nav_msgs::msg::Path noisey_path_refined = noisey_path_baseline;
|
||||
EXPECT_TRUE(smoother->smooth(noisey_path_refined, max_time));
|
||||
|
||||
length = 0;
|
||||
for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) {
|
||||
length += std::hypot(
|
||||
noisey_path_refined.poses[i + 1].pose.position.x -
|
||||
noisey_path_refined.poses[i].pose.position.x,
|
||||
noisey_path_refined.poses[i + 1].pose.position.y -
|
||||
noisey_path_refined.poses[i].pose.position.y);
|
||||
// std::hypot(
|
||||
// noisey_path.poses[i + 1].pose.position.x - noisey_path_baseline.poses[i].pose.position.x,
|
||||
// noisey_path.poses[i + 1].pose.position.y - noisey_path_baseline.poses[i].pose.position.y);
|
||||
}
|
||||
|
||||
EXPECT_LT(length, base_length);
|
||||
}
|
||||
|
||||
TEST(SmootherTest, test_sg_smoother_reversing)
|
||||
{
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr node =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSGSmootherTest");
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false));
|
||||
auto smoother = std::make_unique<nav2_smoother::SavitzkyGolaySmoother>();
|
||||
smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint);
|
||||
rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds
|
||||
|
||||
// Test reversing / multiple segments via a cusp
|
||||
nav_msgs::msg::Path cusp_path, cusp_path_baseline;
|
||||
cusp_path.header.frame_id = "map";
|
||||
cusp_path.header.stamp = node->now();
|
||||
cusp_path.poses.resize(22);
|
||||
cusp_path.poses[0].pose.position.x = 0.5;
|
||||
cusp_path.poses[0].pose.position.y = 0.1;
|
||||
cusp_path.poses[1].pose.position.x = 0.5;
|
||||
cusp_path.poses[1].pose.position.y = 0.2;
|
||||
cusp_path.poses[2].pose.position.x = 0.5;
|
||||
cusp_path.poses[2].pose.position.y = 0.3;
|
||||
cusp_path.poses[3].pose.position.x = 0.5;
|
||||
cusp_path.poses[3].pose.position.y = 0.4;
|
||||
cusp_path.poses[4].pose.position.x = 0.5;
|
||||
cusp_path.poses[4].pose.position.y = 0.5;
|
||||
cusp_path.poses[5].pose.position.x = 0.5;
|
||||
cusp_path.poses[5].pose.position.y = 0.6;
|
||||
cusp_path.poses[6].pose.position.x = 0.5;
|
||||
cusp_path.poses[6].pose.position.y = 0.7;
|
||||
cusp_path.poses[7].pose.position.x = 0.5;
|
||||
cusp_path.poses[7].pose.position.y = 0.8;
|
||||
cusp_path.poses[8].pose.position.x = 0.5;
|
||||
cusp_path.poses[8].pose.position.y = 0.9;
|
||||
cusp_path.poses[9].pose.position.x = 0.5;
|
||||
cusp_path.poses[9].pose.position.y = 1.0;
|
||||
cusp_path.poses[10].pose.position.x = 0.5;
|
||||
cusp_path.poses[10].pose.position.y = 1.1;
|
||||
cusp_path.poses[11].pose.position.x = 0.5;
|
||||
cusp_path.poses[11].pose.position.y = 1.0;
|
||||
cusp_path.poses[12].pose.position.x = 0.5;
|
||||
cusp_path.poses[12].pose.position.y = 0.9;
|
||||
cusp_path.poses[13].pose.position.x = 0.5;
|
||||
cusp_path.poses[13].pose.position.y = 0.8;
|
||||
cusp_path.poses[14].pose.position.x = 0.5;
|
||||
cusp_path.poses[14].pose.position.y = 0.7;
|
||||
cusp_path.poses[15].pose.position.x = 0.5;
|
||||
cusp_path.poses[15].pose.position.y = 0.6;
|
||||
cusp_path.poses[16].pose.position.x = 0.5;
|
||||
cusp_path.poses[16].pose.position.y = 0.5;
|
||||
cusp_path.poses[17].pose.position.x = 0.5;
|
||||
cusp_path.poses[17].pose.position.y = 0.4;
|
||||
cusp_path.poses[18].pose.position.x = 0.5;
|
||||
cusp_path.poses[18].pose.position.y = 0.3;
|
||||
cusp_path.poses[19].pose.position.x = 0.5;
|
||||
cusp_path.poses[19].pose.position.y = 0.2;
|
||||
cusp_path.poses[20].pose.position.x = 0.5;
|
||||
cusp_path.poses[20].pose.position.y = 0.1;
|
||||
cusp_path.poses[21].pose.position.x = 0.5;
|
||||
cusp_path.poses[21].pose.position.y = 0.0;
|
||||
|
||||
// Add random but deterministic noises
|
||||
std::random_device rd{};
|
||||
std::mt19937 gen{rd()};
|
||||
std::normal_distribution<> normal_distribution{0.0, 0.02};
|
||||
for (unsigned int i = 0; i != cusp_path.poses.size(); i++) {
|
||||
auto noise = normal_distribution(gen);
|
||||
cusp_path.poses[i].pose.position.x += noise;
|
||||
}
|
||||
|
||||
cusp_path_baseline = cusp_path;
|
||||
|
||||
EXPECT_TRUE(smoother->smooth(cusp_path, max_time));
|
||||
|
||||
// If it detected the cusp, the cusp point should be fixed
|
||||
EXPECT_EQ(cusp_path.poses[10].pose.position.x, cusp_path_baseline.poses[10].pose.position.x);
|
||||
EXPECT_EQ(cusp_path.poses[10].pose.position.y, cusp_path_baseline.poses[10].pose.position.y);
|
||||
|
||||
// But the path also should be smoother / shorter
|
||||
double length = 0;
|
||||
double base_length = 0;
|
||||
for (unsigned int i = 0; i != cusp_path.poses.size() - 1; i++) {
|
||||
length += std::hypot(
|
||||
cusp_path.poses[i + 1].pose.position.x - cusp_path.poses[i].pose.position.x,
|
||||
cusp_path.poses[i + 1].pose.position.y - cusp_path.poses[i].pose.position.y);
|
||||
base_length += std::hypot(
|
||||
cusp_path_baseline.poses[i + 1].pose.position.x -
|
||||
cusp_path_baseline.poses[i].pose.position.x,
|
||||
cusp_path_baseline.poses[i + 1].pose.position.y -
|
||||
cusp_path_baseline.poses[i].pose.position.y);
|
||||
}
|
||||
|
||||
EXPECT_LT(length, base_length);
|
||||
}
|
||||
@@ -0,0 +1,280 @@
|
||||
// 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));
|
||||
}
|
||||
@@ -0,0 +1,440 @@
|
||||
// Copyright (c) 2021 RoboTech Vision
|
||||
//
|
||||
// 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 <memory>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <future>
|
||||
#include <thread>
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "nav2_core/smoother.hpp"
|
||||
#include "nav2_core/exceptions.hpp"
|
||||
#include "nav2_msgs/action/smooth_path.hpp"
|
||||
#include "nav2_smoother/nav2_smoother.hpp"
|
||||
#include "tf2_ros/create_timer_ros.h"
|
||||
|
||||
using SmoothAction = nav2_msgs::action::SmoothPath;
|
||||
using ClientGoalHandle = rclcpp_action::ClientGoalHandle<SmoothAction>;
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// A smoother for testing the base class
|
||||
|
||||
class DummySmoother : public nav2_core::Smoother
|
||||
{
|
||||
public:
|
||||
DummySmoother() {}
|
||||
|
||||
~DummySmoother() {}
|
||||
|
||||
virtual void configure(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr &,
|
||||
std::string, std::shared_ptr<tf2_ros::Buffer>,
|
||||
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>,
|
||||
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>) {}
|
||||
|
||||
virtual void cleanup() {}
|
||||
|
||||
virtual void activate() {}
|
||||
|
||||
virtual void deactivate() {}
|
||||
|
||||
virtual bool smooth(
|
||||
nav_msgs::msg::Path & path,
|
||||
const rclcpp::Duration & max_time)
|
||||
{
|
||||
assert(path.poses.size() == 2);
|
||||
|
||||
if (path.poses.front() == path.poses.back()) {
|
||||
throw nav2_core::PlannerException("Start and goal pose must differ");
|
||||
}
|
||||
|
||||
auto max_time_ms = max_time.to_chrono<std::chrono::milliseconds>();
|
||||
std::this_thread::sleep_for(std::min(max_time_ms, 100ms));
|
||||
|
||||
// place dummy pose in the middle of the path
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.pose.position.x =
|
||||
(path.poses.front().pose.position.x + path.poses.back().pose.position.x) / 2;
|
||||
pose.pose.position.y =
|
||||
(path.poses.front().pose.position.y + path.poses.back().pose.position.y) / 2;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
path.poses.push_back(pose);
|
||||
|
||||
return max_time_ms > 100ms;
|
||||
}
|
||||
|
||||
private:
|
||||
std::string command_;
|
||||
std::chrono::system_clock::time_point start_time_;
|
||||
};
|
||||
|
||||
// Mocked class loader
|
||||
void onPluginDeletion(nav2_core::Smoother * obj)
|
||||
{
|
||||
if (nullptr != obj) {
|
||||
delete (obj);
|
||||
}
|
||||
}
|
||||
|
||||
template<>
|
||||
pluginlib::UniquePtr<nav2_core::Smoother> pluginlib::ClassLoader<nav2_core::Smoother>::
|
||||
createUniqueInstance(const std::string & lookup_name)
|
||||
{
|
||||
if (lookup_name != "DummySmoother") {
|
||||
// original method body
|
||||
if (!isClassLoaded(lookup_name)) {
|
||||
loadLibraryForClass(lookup_name);
|
||||
}
|
||||
try {
|
||||
std::string class_type = getClassType(lookup_name);
|
||||
pluginlib::UniquePtr<nav2_core::Smoother> obj =
|
||||
lowlevel_class_loader_.createUniqueInstance<nav2_core::Smoother>(class_type);
|
||||
return obj;
|
||||
} catch (const class_loader::CreateClassException & ex) {
|
||||
throw pluginlib::CreateClassException(ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
// mocked plugin creation
|
||||
return std::unique_ptr<nav2_core::Smoother,
|
||||
class_loader::ClassLoader::DeleterType<nav2_core::Smoother>>(
|
||||
new DummySmoother(),
|
||||
onPluginDeletion);
|
||||
}
|
||||
|
||||
class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
|
||||
{
|
||||
public:
|
||||
DummyCostmapSubscriber(
|
||||
nav2_util::LifecycleNode::SharedPtr node,
|
||||
const std::string & topic_name)
|
||||
: CostmapSubscriber(node, topic_name)
|
||||
{
|
||||
auto costmap = std::make_shared<nav2_msgs::msg::Costmap>();
|
||||
costmap->metadata.size_x = 100;
|
||||
costmap->metadata.size_y = 100;
|
||||
costmap->metadata.resolution = 0.1;
|
||||
costmap->metadata.origin.position.x = -5.0;
|
||||
costmap->metadata.origin.position.y = -5.0;
|
||||
|
||||
costmap->data.resize(costmap->metadata.size_x * costmap->metadata.size_y, 0);
|
||||
for (unsigned int i = 0; i < costmap->metadata.size_y; ++i) {
|
||||
for (unsigned int j = 20; j < 40; ++j) {
|
||||
costmap->data[i * costmap->metadata.size_x + j] = 254;
|
||||
}
|
||||
}
|
||||
|
||||
setCostmap(costmap);
|
||||
}
|
||||
|
||||
void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg)
|
||||
{
|
||||
costmap_msg_ = msg;
|
||||
costmap_received_ = true;
|
||||
}
|
||||
};
|
||||
|
||||
class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber
|
||||
{
|
||||
public:
|
||||
DummyFootprintSubscriber(
|
||||
nav2_util::LifecycleNode::SharedPtr node,
|
||||
const std::string & topic_name,
|
||||
tf2_ros::Buffer & tf_)
|
||||
: FootprintSubscriber(node, topic_name, tf_)
|
||||
{
|
||||
auto footprint = std::make_shared<geometry_msgs::msg::PolygonStamped>();
|
||||
footprint->header.frame_id = "base_link"; // global frame = robot frame to avoid tf lookup
|
||||
footprint->header.stamp = node->get_clock()->now();
|
||||
geometry_msgs::msg::Point32 point;
|
||||
point.x = -0.2f;
|
||||
point.y = -0.2f;
|
||||
footprint->polygon.points.push_back(point);
|
||||
point.y = 0.2f;
|
||||
footprint->polygon.points.push_back(point);
|
||||
point.x = 0.2f;
|
||||
point.y = 0.0f;
|
||||
footprint->polygon.points.push_back(point);
|
||||
|
||||
setFootprint(footprint);
|
||||
}
|
||||
|
||||
void setFootprint(geometry_msgs::msg::PolygonStamped::SharedPtr msg)
|
||||
{
|
||||
footprint_ = msg;
|
||||
footprint_received_ = true;
|
||||
}
|
||||
};
|
||||
|
||||
class DummySmootherServer : public nav2_smoother::SmootherServer
|
||||
{
|
||||
public:
|
||||
DummySmootherServer()
|
||||
{
|
||||
// Override defaults
|
||||
default_ids_.clear();
|
||||
default_ids_.resize(1, "SmoothPath");
|
||||
set_parameter(rclcpp::Parameter("smoother_plugins", default_ids_));
|
||||
default_types_.clear();
|
||||
default_types_.resize(1, "DummySmoother");
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn
|
||||
on_configure(const rclcpp_lifecycle::State & state)
|
||||
{
|
||||
auto result = SmootherServer::on_configure(state);
|
||||
if (result != nav2_util::CallbackReturn::SUCCESS) {
|
||||
return result;
|
||||
}
|
||||
|
||||
// Create dummy subscribers and collision checker
|
||||
auto node = shared_from_this();
|
||||
costmap_sub_ =
|
||||
std::make_shared<DummyCostmapSubscriber>(
|
||||
node, "costmap_topic");
|
||||
footprint_sub_ =
|
||||
std::make_shared<DummyFootprintSubscriber>(
|
||||
node, "footprint_topic", *tf_);
|
||||
collision_checker_ =
|
||||
std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
|
||||
*costmap_sub_, *footprint_sub_,
|
||||
node->get_name());
|
||||
|
||||
return result;
|
||||
}
|
||||
};
|
||||
|
||||
// Define a test class to hold the context for the tests
|
||||
class SmootherTest : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
SmootherTest() {}
|
||||
~SmootherTest() {}
|
||||
|
||||
void SetUp() override
|
||||
{
|
||||
node_ =
|
||||
std::make_shared<rclcpp::Node>(
|
||||
"LifecycleSmootherTestNode", rclcpp::NodeOptions());
|
||||
|
||||
smoother_server_ = std::make_shared<DummySmootherServer>();
|
||||
smoother_server_->set_parameter(
|
||||
rclcpp::Parameter(
|
||||
"smoother_plugins",
|
||||
rclcpp::ParameterValue(std::vector<std::string>(1, "DummySmoothPath"))));
|
||||
smoother_server_->declare_parameter(
|
||||
"DummySmoothPath.plugin",
|
||||
rclcpp::ParameterValue(std::string("DummySmoother")));
|
||||
smoother_server_->configure();
|
||||
smoother_server_->activate();
|
||||
|
||||
client_ = rclcpp_action::create_client<SmoothAction>(
|
||||
node_->get_node_base_interface(),
|
||||
node_->get_node_graph_interface(),
|
||||
node_->get_node_logging_interface(),
|
||||
node_->get_node_waitables_interface(), "smooth_path");
|
||||
std::cout << "Setup complete." << std::endl;
|
||||
}
|
||||
|
||||
void TearDown() override
|
||||
{
|
||||
smoother_server_->deactivate();
|
||||
smoother_server_->cleanup();
|
||||
smoother_server_->shutdown();
|
||||
smoother_server_.reset();
|
||||
client_.reset();
|
||||
node_.reset();
|
||||
}
|
||||
|
||||
bool sendGoal(
|
||||
std::string smoother_id, double x_start, double y_start, double x_goal,
|
||||
double y_goal, std::chrono::milliseconds max_time, bool check_for_collisions)
|
||||
{
|
||||
if (!client_->wait_for_action_server(4s)) {
|
||||
std::cout << "Server not up" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
|
||||
auto goal = SmoothAction::Goal();
|
||||
goal.smoother_id = smoother_id;
|
||||
pose.pose.position.x = x_start;
|
||||
pose.pose.position.y = y_start;
|
||||
goal.path.poses.push_back(pose);
|
||||
pose.pose.position.x = x_goal;
|
||||
pose.pose.position.y = y_goal;
|
||||
goal.path.poses.push_back(pose);
|
||||
goal.check_for_collisions = check_for_collisions;
|
||||
goal.max_smoothing_duration = rclcpp::Duration(max_time);
|
||||
|
||||
auto future_goal = client_->async_send_goal(goal);
|
||||
|
||||
if (rclcpp::spin_until_future_complete(node_, future_goal) !=
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
std::cout << "failed sending goal" << std::endl;
|
||||
// failed sending the goal
|
||||
return false;
|
||||
}
|
||||
|
||||
goal_handle_ = future_goal.get();
|
||||
|
||||
if (!goal_handle_) {
|
||||
std::cout << "goal was rejected" << std::endl;
|
||||
// goal was rejected by the action server
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
ClientGoalHandle::WrappedResult getResult()
|
||||
{
|
||||
std::cout << "Getting async result..." << std::endl;
|
||||
auto future_result = client_->async_get_result(goal_handle_);
|
||||
std::cout << "Waiting on future..." << std::endl;
|
||||
rclcpp::spin_until_future_complete(node_, future_result);
|
||||
std::cout << "future received!" << std::endl;
|
||||
return future_result.get();
|
||||
}
|
||||
|
||||
std::shared_ptr<rclcpp::Node> node_;
|
||||
std::shared_ptr<DummySmootherServer> smoother_server_;
|
||||
std::shared_ptr<rclcpp_action::Client<SmoothAction>> client_;
|
||||
std::shared_ptr<rclcpp_action::ClientGoalHandle<SmoothAction>> goal_handle_;
|
||||
};
|
||||
|
||||
// Define the tests
|
||||
|
||||
TEST_F(SmootherTest, testingSuccess)
|
||||
{
|
||||
ASSERT_TRUE(sendGoal("DummySmoothPath", 0.0, 0.0, 1.0, 0.0, 500ms, true));
|
||||
auto result = getResult();
|
||||
EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
|
||||
EXPECT_EQ(result.result->path.poses.size(), (std::size_t)3);
|
||||
EXPECT_TRUE(result.result->was_completed);
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST_F(SmootherTest, testingFailureOnInvalidSmootherId)
|
||||
{
|
||||
ASSERT_TRUE(sendGoal("InvalidSmoother", 0.0, 0.0, 1.0, 0.0, 500ms, true));
|
||||
auto result = getResult();
|
||||
EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED);
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST_F(SmootherTest, testingSuccessOnEmptyPlugin)
|
||||
{
|
||||
ASSERT_TRUE(sendGoal("", 0.0, 0.0, 1.0, 0.0, 500ms, true));
|
||||
auto result = getResult();
|
||||
EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST_F(SmootherTest, testingIncomplete)
|
||||
{
|
||||
ASSERT_TRUE(sendGoal("DummySmoothPath", 0.0, 0.0, 1.0, 0.0, 50ms, true));
|
||||
auto result = getResult();
|
||||
EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
|
||||
EXPECT_FALSE(result.result->was_completed);
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST_F(SmootherTest, testingFailureOnException)
|
||||
{
|
||||
ASSERT_TRUE(sendGoal("DummySmoothPath", 0.0, 0.0, 0.0, 0.0, 500ms, true));
|
||||
auto result = getResult();
|
||||
EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED);
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST_F(SmootherTest, testingFailureOnCollision)
|
||||
{
|
||||
ASSERT_TRUE(sendGoal("DummySmoothPath", -4.0, 0.0, 0.0, 0.0, 500ms, true));
|
||||
auto result = getResult();
|
||||
EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED);
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST_F(SmootherTest, testingCollisionCheckDisabled)
|
||||
{
|
||||
ASSERT_TRUE(sendGoal("DummySmoothPath", -4.0, 0.0, 0.0, 0.0, 500ms, false));
|
||||
auto result = getResult();
|
||||
EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin)
|
||||
{
|
||||
auto smoother_server = std::make_shared<DummySmootherServer>();
|
||||
smoother_server->set_parameter(
|
||||
rclcpp::Parameter(
|
||||
"smoother_plugins",
|
||||
rclcpp::ParameterValue(std::vector<std::string>(1, "DummySmoothPath"))));
|
||||
smoother_server->declare_parameter(
|
||||
"DummySmoothPath.plugin",
|
||||
rclcpp::ParameterValue(std::string("DummySmoother")));
|
||||
auto state = smoother_server->configure();
|
||||
EXPECT_EQ(state.id(), 2); // 1 on failure, 2 on success
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin)
|
||||
{
|
||||
auto smoother_server = std::make_shared<DummySmootherServer>();
|
||||
smoother_server->set_parameter(
|
||||
rclcpp::Parameter(
|
||||
"smoother_plugins",
|
||||
rclcpp::ParameterValue(std::vector<std::string>(1, "DummySmoothPath"))));
|
||||
smoother_server->declare_parameter(
|
||||
"DummySmoothPath.plugin",
|
||||
rclcpp::ParameterValue(std::string("InvalidSmootherPlugin")));
|
||||
auto state = smoother_server->configure();
|
||||
EXPECT_EQ(state.id(), 1); // 1 on failure, 2 on success
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin)
|
||||
{
|
||||
auto smoother_server = std::make_shared<DummySmootherServer>();
|
||||
auto state = smoother_server->configure();
|
||||
EXPECT_EQ(state.id(), 2); // 1 on failure, 2 on success
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
Reference in New Issue
Block a user