add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,14 @@
|
||||
ament_add_gtest(prefer_forward_tests prefer_forward_test.cpp)
|
||||
target_link_libraries(prefer_forward_tests dwb_critics)
|
||||
|
||||
ament_add_gtest(base_obstacle_tests base_obstacle_test.cpp)
|
||||
target_link_libraries(base_obstacle_tests dwb_critics)
|
||||
|
||||
ament_add_gtest(obstacle_footprint_tests obstacle_footprint_test.cpp)
|
||||
target_link_libraries(obstacle_footprint_tests dwb_critics)
|
||||
|
||||
ament_add_gtest(alignment_util_tests alignment_util_test.cpp)
|
||||
target_link_libraries(alignment_util_tests dwb_critics)
|
||||
|
||||
ament_add_gtest(twirling_tests twirling_test.cpp)
|
||||
target_link_libraries(twirling_tests dwb_critics)
|
||||
@@ -0,0 +1,77 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2020, Samsung Research America
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dwb_critics/alignment_util.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
|
||||
TEST(AlignmentUtil, TestProjection)
|
||||
{
|
||||
geometry_msgs::msg::Pose2D pose, pose_out;
|
||||
pose.x = 1.0;
|
||||
pose.y = -1.0;
|
||||
double distance = 1.0;
|
||||
pose_out = dwb_critics::getForwardPose(pose, distance);
|
||||
EXPECT_EQ(pose_out.x, 2.0);
|
||||
EXPECT_EQ(pose_out.y, -1.0);
|
||||
EXPECT_EQ(pose_out.theta, pose.theta);
|
||||
|
||||
pose.x = 2.0;
|
||||
pose.y = -10.0;
|
||||
pose.theta = 0.54;
|
||||
pose_out = dwb_critics::getForwardPose(pose, distance);
|
||||
EXPECT_NEAR(pose_out.x, 2.8577, 0.01);
|
||||
EXPECT_NEAR(pose_out.y, -9.4858, 0.01);
|
||||
EXPECT_EQ(pose_out.theta, pose.theta);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,173 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Wilco Bonestroo
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dwb_critics/obstacle_footprint.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
|
||||
TEST(BaseObstacle, IsValidCost)
|
||||
{
|
||||
std::shared_ptr<dwb_critics::BaseObstacleCritic> critic =
|
||||
std::make_shared<dwb_critics::BaseObstacleCritic>();
|
||||
|
||||
for (int i = 0; i < 256; i++) {
|
||||
// for these 3 values the cost is not "valid"
|
||||
if (i == nav2_costmap_2d::LETHAL_OBSTACLE ||
|
||||
i == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
|
||||
i == nav2_costmap_2d::NO_INFORMATION)
|
||||
{
|
||||
ASSERT_FALSE(critic->isValidCost(i));
|
||||
} else {
|
||||
ASSERT_TRUE(critic->isValidCost(i));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(BaseObstacle, ScorePose)
|
||||
{
|
||||
std::shared_ptr<dwb_critics::BaseObstacleCritic> critic =
|
||||
std::make_shared<dwb_critics::BaseObstacleCritic>();
|
||||
|
||||
auto node = nav2_util::LifecycleNode::make_shared("base_obstacle_critic_tester");
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
costmap_ros->configure();
|
||||
|
||||
std::string name = "name";
|
||||
std::string ns = "ns";
|
||||
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
costmap_ros->getCostmap()->setCost(0, 0, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
costmap_ros->getCostmap()->setCost(0, 1, nav2_costmap_2d::NO_INFORMATION);
|
||||
const int some_other_cost = 128;
|
||||
costmap_ros->getCostmap()->setCost(0, 2, some_other_cost);
|
||||
|
||||
// The pose is in "world" coordinates. The (default) resolution is 0.1 m.
|
||||
geometry_msgs::msg::Pose2D pose;
|
||||
pose.x = 0;
|
||||
pose.y = 0;
|
||||
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = 0;
|
||||
pose.y = 0.15;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.y = 0.25;
|
||||
pose.x = 0.05;
|
||||
ASSERT_EQ(critic->scorePose(pose), some_other_cost);
|
||||
|
||||
// The theta should not influence the cost
|
||||
for (int i = -50; i < 150; i++) {
|
||||
pose.theta = (1.0 / 50) * i * M_PI;
|
||||
ASSERT_EQ(critic->scorePose(pose), some_other_cost);
|
||||
}
|
||||
|
||||
// Poses outside the map should throw an exception.
|
||||
pose.x = 1.0;
|
||||
pose.y = -0.1;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = costmap_ros->getCostmap()->getSizeInMetersX() + 0.1;
|
||||
pose.y = 1.0;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = 1.0;
|
||||
pose.y = costmap_ros->getCostmap()->getSizeInMetersY() + 0.1;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = -0.1;
|
||||
pose.y = 1.0;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
}
|
||||
|
||||
TEST(BaseObstacle, CriticVisualization)
|
||||
{
|
||||
std::shared_ptr<dwb_critics::BaseObstacleCritic> critic =
|
||||
std::make_shared<dwb_critics::BaseObstacleCritic>();
|
||||
|
||||
auto node = nav2_util::LifecycleNode::make_shared("base_obstacle_critic_tester");
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
costmap_ros->configure();
|
||||
|
||||
std::string name = "name";
|
||||
std::string ns = "ns";
|
||||
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
costmap_ros->getCostmap()->setCost(0, 0, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
costmap_ros->getCostmap()->setCost(0, 1, nav2_costmap_2d::NO_INFORMATION);
|
||||
// Some random values
|
||||
costmap_ros->getCostmap()->setCost(3, 2, 64);
|
||||
costmap_ros->getCostmap()->setCost(30, 12, 85);
|
||||
costmap_ros->getCostmap()->setCost(10, 49, 24);
|
||||
costmap_ros->getCostmap()->setCost(45, 2, 12);
|
||||
|
||||
std::vector<std::pair<std::string, std::vector<float>>> cost_channels;
|
||||
critic->addCriticVisualization(cost_channels);
|
||||
|
||||
unsigned int size_x = costmap_ros->getCostmap()->getSizeInCellsX();
|
||||
unsigned int size_y = costmap_ros->getCostmap()->getSizeInCellsY();
|
||||
|
||||
// The values in the pointcloud should be equal to the values in the costmap
|
||||
for (unsigned int y = 0; y < size_y; y++) {
|
||||
for (unsigned int x = 0; x < size_x; x++) {
|
||||
float pointValue = cost_channels[0].second[y * size_y + x];
|
||||
ASSERT_EQ(static_cast<int>(pointValue), costmap_ros->getCostmap()->getCost(x, y));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,264 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Wilco Bonestroo
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dwb_critics/obstacle_footprint.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
|
||||
class OpenObstacleFootprintCritic : public dwb_critics::ObstacleFootprintCritic
|
||||
{
|
||||
public:
|
||||
double pointCost(int x, int y)
|
||||
{
|
||||
return dwb_critics::ObstacleFootprintCritic::pointCost(x, y);
|
||||
}
|
||||
|
||||
double lineCost(int x0, int x1, int y0, int y1)
|
||||
{
|
||||
return dwb_critics::ObstacleFootprintCritic::lineCost(x0, x1, y0, y1);
|
||||
}
|
||||
};
|
||||
|
||||
// Rotate the given point for angle radians around the origin.
|
||||
geometry_msgs::msg::Point rotate_origin(geometry_msgs::msg::Point p, double angle)
|
||||
{
|
||||
double s = sin(angle);
|
||||
double c = cos(angle);
|
||||
|
||||
// rotate point
|
||||
double xnew = p.x * c - p.y * s;
|
||||
double ynew = p.x * s + p.y * c;
|
||||
|
||||
p.x = xnew;
|
||||
p.y = ynew;
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
// Auxilary function to create a Point with given x and y values.
|
||||
geometry_msgs::msg::Point getPoint(double x, double y)
|
||||
{
|
||||
geometry_msgs::msg::Point p;
|
||||
p.x = x;
|
||||
p.y = y;
|
||||
return p;
|
||||
}
|
||||
|
||||
// Variables
|
||||
double footprint_size_x_half = 1.8;
|
||||
double footprint_size_y_half = 1.6;
|
||||
|
||||
std::vector<geometry_msgs::msg::Point> getFootprint()
|
||||
{
|
||||
std::vector<geometry_msgs::msg::Point> footprint;
|
||||
footprint.push_back(getPoint(footprint_size_x_half, footprint_size_y_half));
|
||||
footprint.push_back(getPoint(footprint_size_x_half, -footprint_size_y_half));
|
||||
footprint.push_back(getPoint(-footprint_size_x_half, -footprint_size_y_half));
|
||||
footprint.push_back(getPoint(-footprint_size_x_half, footprint_size_y_half));
|
||||
return footprint;
|
||||
}
|
||||
|
||||
TEST(ObstacleFootprint, GetOrientedFootprint)
|
||||
{
|
||||
double theta = 0.1234;
|
||||
|
||||
std::vector<geometry_msgs::msg::Point> footprint_before = getFootprint();
|
||||
|
||||
std::vector<geometry_msgs::msg::Point> footprint_after;
|
||||
geometry_msgs::msg::Pose2D pose;
|
||||
pose.theta = theta;
|
||||
footprint_after = dwb_critics::getOrientedFootprint(pose, footprint_before);
|
||||
|
||||
uint i;
|
||||
for (i = 0; i < footprint_before.size(); i++) {
|
||||
ASSERT_EQ(rotate_origin(footprint_before[i], theta), footprint_after[i]);
|
||||
}
|
||||
|
||||
theta = 5.123;
|
||||
pose.theta = theta;
|
||||
footprint_after = dwb_critics::getOrientedFootprint(pose, footprint_before);
|
||||
|
||||
for (unsigned int i = 0; i < footprint_before.size(); i++) {
|
||||
ASSERT_EQ(rotate_origin(footprint_before[i], theta), footprint_after[i]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(ObstacleFootprint, Prepare)
|
||||
{
|
||||
std::shared_ptr<dwb_critics::ObstacleFootprintCritic> critic =
|
||||
std::make_shared<dwb_critics::ObstacleFootprintCritic>();
|
||||
|
||||
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
costmap_ros->configure();
|
||||
|
||||
std::string name = "name";
|
||||
std::string ns = "ns";
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
geometry_msgs::msg::Pose2D pose;
|
||||
nav_2d_msgs::msg::Twist2D vel;
|
||||
geometry_msgs::msg::Pose2D goal;
|
||||
nav_2d_msgs::msg::Path2D global_plan;
|
||||
|
||||
// no footprint set in the costmap. Prepare should return false;
|
||||
std::vector<geometry_msgs::msg::Point> footprint;
|
||||
costmap_ros->setRobotFootprint(footprint);
|
||||
ASSERT_FALSE(critic->prepare(pose, vel, goal, global_plan));
|
||||
|
||||
costmap_ros->setRobotFootprint(getFootprint());
|
||||
ASSERT_TRUE(critic->prepare(pose, vel, goal, global_plan));
|
||||
|
||||
double epsilon = 0.01;
|
||||
// If the robot footprint goes of the map, it should throw an exception
|
||||
// The following cases put the robot over the edge of the map on the left, bottom, right and top
|
||||
|
||||
pose.x = footprint_size_x_half; // This gives an error
|
||||
pose.y = footprint_size_y_half + epsilon;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = footprint_size_x_half + epsilon;
|
||||
pose.y = footprint_size_y_half; // error
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = costmap_ros->getCostmap()->getSizeInMetersX() - footprint_size_x_half; // error
|
||||
pose.y = costmap_ros->getCostmap()->getSizeInMetersY() + footprint_size_y_half - epsilon;
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = costmap_ros->getCostmap()->getSizeInMetersX() - footprint_size_x_half - epsilon;
|
||||
pose.y = costmap_ros->getCostmap()->getSizeInMetersY() + footprint_size_y_half; // error
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
pose.x = footprint_size_x_half + epsilon;
|
||||
pose.y = footprint_size_y_half + epsilon;
|
||||
ASSERT_EQ(critic->scorePose(pose), 0.0);
|
||||
|
||||
for (unsigned int i = 1; i < costmap_ros->getCostmap()->getSizeInCellsX(); i++) {
|
||||
costmap_ros->getCostmap()->setCost(i, 10, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
}
|
||||
// It should now hit an obstacle (throw an expection)
|
||||
ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException);
|
||||
}
|
||||
|
||||
// todo: wilcobonestroo Add tests for other footprint shapes and costmaps.
|
||||
|
||||
TEST(ObstacleFootprint, PointCost)
|
||||
{
|
||||
std::shared_ptr<OpenObstacleFootprintCritic> critic =
|
||||
std::make_shared<OpenObstacleFootprintCritic>();
|
||||
|
||||
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
costmap_ros->configure();
|
||||
|
||||
std::string name = "name";
|
||||
std::string ns = "ns";
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
costmap_ros->getCostmap()->setCost(0, 0, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
costmap_ros->getCostmap()->setCost(0, 1, nav2_costmap_2d::NO_INFORMATION);
|
||||
costmap_ros->getCostmap()->setCost(0, 2, 128);
|
||||
|
||||
ASSERT_THROW(critic->pointCost(0, 0), dwb_core::IllegalTrajectoryException);
|
||||
ASSERT_THROW(critic->pointCost(0, 1), dwb_core::IllegalTrajectoryException);
|
||||
ASSERT_EQ(critic->pointCost(0, 2), 128);
|
||||
}
|
||||
|
||||
TEST(ObstacleFootprint, LineCost)
|
||||
{
|
||||
std::shared_ptr<OpenObstacleFootprintCritic> critic =
|
||||
std::make_shared<OpenObstacleFootprintCritic>();
|
||||
|
||||
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
costmap_ros->configure();
|
||||
|
||||
std::string name = "name";
|
||||
std::string ns = "ns";
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
costmap_ros->getCostmap()->setCost(3, 3, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
costmap_ros->getCostmap()->setCost(3, 4, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
costmap_ros->getCostmap()->setCost(4, 3, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
costmap_ros->getCostmap()->setCost(4, 4, nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||
|
||||
ASSERT_THROW(critic->lineCost(0, 5, 2, 6), dwb_core::IllegalTrajectoryException);
|
||||
ASSERT_THROW(critic->lineCost(5, 0, 6, 2), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
ASSERT_THROW(critic->lineCost(2, 4, 0, 10), dwb_core::IllegalTrajectoryException);
|
||||
ASSERT_THROW(critic->lineCost(4, 2, 10, 0), dwb_core::IllegalTrajectoryException);
|
||||
|
||||
// These all miss the obstacle
|
||||
ASSERT_EQ(critic->lineCost(2, 2, 0, 10), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(2, 2, 10, 0), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(5, 5, 0, 10), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(5, 5, 10, 0), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(0, 50, 2, 2), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(50, 0, 2, 2), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(0, 50, 5, 5), 0.0);
|
||||
ASSERT_EQ(critic->lineCost(50, 0, 5, 5), 0.0);
|
||||
|
||||
// Use valid costs
|
||||
costmap_ros->getCostmap()->setCost(3, 3, 50);
|
||||
costmap_ros->getCostmap()->setCost(3, 4, 50);
|
||||
costmap_ros->getCostmap()->setCost(4, 3, 100);
|
||||
costmap_ros->getCostmap()->setCost(4, 4, 100);
|
||||
|
||||
ASSERT_EQ(critic->lineCost(3, 3, 0, 50), 50); // all 50
|
||||
ASSERT_EQ(critic->lineCost(4, 4, 0, 10), 100); // all 100
|
||||
ASSERT_EQ(critic->lineCost(0, 50, 3, 3), 100); // pass 50 and 100
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,229 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Wilco Bonestroo
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <vector>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dwb_critics/prefer_forward.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
static constexpr double default_penalty = 1.0;
|
||||
static constexpr double default_strafe_x = 0.1;
|
||||
static constexpr double default_strafe_theta = 0.2;
|
||||
static constexpr double default_theta_scale = 10.0;
|
||||
|
||||
TEST(PreferForward, StartNode)
|
||||
{
|
||||
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>();
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
|
||||
node->configure();
|
||||
node->activate();
|
||||
|
||||
std::string name = "test";
|
||||
std::string ns = "ns";
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
EXPECT_EQ(node->get_parameter(ns + "." + name + ".penalty").as_double(), default_penalty);
|
||||
EXPECT_EQ(node->get_parameter(ns + "." + name + ".strafe_x").as_double(), default_strafe_x);
|
||||
EXPECT_EQ(
|
||||
node->get_parameter(ns + "." + name + ".strafe_theta").as_double(), default_strafe_theta);
|
||||
EXPECT_EQ(node->get_parameter(ns + "." + name + ".theta_scale").as_double(), default_theta_scale);
|
||||
}
|
||||
|
||||
TEST(PreferForward, NegativeVelocityX)
|
||||
{
|
||||
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>();
|
||||
dwb_msgs::msg::Trajectory2D trajectory;
|
||||
|
||||
// score must be equal to the penalty (1.0) for any negative x velocity
|
||||
trajectory.velocity.x = -1.0;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.x = -0.00001;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.x = -0.1;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.x = std::numeric_limits<double>::lowest();
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.x = -std::numeric_limits<double>::min();
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
}
|
||||
|
||||
TEST(PreferForward, Strafe)
|
||||
{
|
||||
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>();
|
||||
dwb_msgs::msg::Trajectory2D trajectory;
|
||||
|
||||
// score must be equal to the penalty (1.0) when x vel is lower than 0.1
|
||||
// and theta is between -0.2 and 0.2
|
||||
trajectory.velocity.x = 0.05;
|
||||
trajectory.velocity.theta = -0.1;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.x = 0.0999999;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.x = 0.000001;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.theta = -0.19;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
|
||||
trajectory.velocity.theta = 0.19;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty);
|
||||
}
|
||||
|
||||
TEST(PreferForward, Normal)
|
||||
{
|
||||
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>();
|
||||
dwb_msgs::msg::Trajectory2D trajectory;
|
||||
|
||||
// score must be equal to the theta * scaling factor (10.0)
|
||||
trajectory.velocity.x = 0.2;
|
||||
trajectory.velocity.theta = -0.1;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.1 * default_theta_scale);
|
||||
|
||||
trajectory.velocity.theta = 0.1;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.1 * default_theta_scale);
|
||||
|
||||
trajectory.velocity.theta = -0.2;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.2 * default_theta_scale);
|
||||
|
||||
trajectory.velocity.theta = 0.2;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.2 * default_theta_scale);
|
||||
|
||||
trajectory.velocity.theta = 1.5;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 1.5 * default_theta_scale);
|
||||
}
|
||||
|
||||
TEST(PreferForward, NoneDefaultValues)
|
||||
{
|
||||
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>();
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
|
||||
node->configure();
|
||||
node->activate();
|
||||
|
||||
double penalty = 18.3;
|
||||
double strafe_x = 0.5;
|
||||
double strafe_theta = 0.4;
|
||||
double theta_scale = 15.0;
|
||||
|
||||
std::string name = "test";
|
||||
std::string ns = "ns";
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, ns + "." + name + ".penalty",
|
||||
rclcpp::ParameterValue(penalty));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, ns + "." + name + ".strafe_x",
|
||||
rclcpp::ParameterValue(strafe_x));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, ns + "." + name + ".strafe_theta",
|
||||
rclcpp::ParameterValue(strafe_theta));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, ns + "." + name + ".theta_scale",
|
||||
rclcpp::ParameterValue(theta_scale));
|
||||
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
critic->onInit();
|
||||
|
||||
dwb_msgs::msg::Trajectory2D trajectory;
|
||||
trajectory.velocity.x = 0.05;
|
||||
trajectory.velocity.theta = -0.1;
|
||||
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty);
|
||||
|
||||
// score must be equal to the penalty when x vel is lower than strafe_x
|
||||
// and theta is between -strafe_theta and strafe_theta
|
||||
trajectory.velocity.x = 0.4;
|
||||
trajectory.velocity.theta = -0.39;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty);
|
||||
|
||||
trajectory.velocity.x = 0.0999999;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty);
|
||||
|
||||
trajectory.velocity.x = 0.000001;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty);
|
||||
|
||||
trajectory.velocity.theta = -0.09999;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty);
|
||||
|
||||
trajectory.velocity.theta = 0.09999;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty);
|
||||
|
||||
// score must be equal to the theta * scaling factor (10.0)
|
||||
trajectory.velocity.x = 0.5;
|
||||
trajectory.velocity.theta = -0.1;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.1 * theta_scale);
|
||||
|
||||
trajectory.velocity.theta = 0.1;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.1 * theta_scale);
|
||||
|
||||
trajectory.velocity.theta = -0.2;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.2 * theta_scale);
|
||||
|
||||
trajectory.velocity.theta = 0.2;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.2 * theta_scale);
|
||||
|
||||
trajectory.velocity.theta = 1.5;
|
||||
EXPECT_EQ(critic->scoreTrajectory(trajectory), 1.5 * theta_scale);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,78 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2020, Samsung Research America
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dwb_critics/twirling.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
|
||||
TEST(TwirlingTests, Scoring)
|
||||
{
|
||||
std::shared_ptr<dwb_critics::TwirlingCritic> critic =
|
||||
std::make_shared<dwb_critics::TwirlingCritic>();
|
||||
|
||||
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_global_costmap");
|
||||
costmap_ros->configure();
|
||||
|
||||
std::string name = "name";
|
||||
std::string ns = "ns";
|
||||
critic->initialize(node, name, ns, costmap_ros);
|
||||
|
||||
dwb_msgs::msg::Trajectory2D traj;
|
||||
traj.velocity.theta = 1.0;
|
||||
EXPECT_EQ(critic->scoreTrajectory(traj), 1.0);
|
||||
traj.velocity.theta = -1.0;
|
||||
EXPECT_EQ(critic->scoreTrajectory(traj), 1.0);
|
||||
}
|
||||
|
||||
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