Skip to content

Commit

Permalink
add tests
Browse files Browse the repository at this point in the history 
Signed-off-by: asarazin <[email protected]>
  • Loading branch information
asarazin committed Aug 23, 2024
1 parent d4be83d commit e53372f
Show file tree
Hide file tree
Showing 2 changed files with 135 additions and 12 deletions.
60 changes: 56 additions & 4 deletions nav2_collision_monitor/test/collision_monitor_node_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,8 @@ class Tester : public ::testing::Test
void setCommonParameters();
void addPolygon(
const std::string & polygon_name, const PolygonType type,
const double size, const std::string & at);
const double size, const std::string & at,
const std::vector<std::string> & sources_names = std::vector<std::string>());
void addPolygonVelocitySubPolygon(
const std::string & polygon_name, const std::string & sub_polygon_name,
const double linear_min, const double linear_max,
Expand Down Expand Up @@ -309,7 +310,8 @@ void Tester::setCommonParameters()

void Tester::addPolygon(
const std::string & polygon_name, const PolygonType type,
const double size, const std::string & at)
const double size, const std::string & at,
const std::vector<std::string> & sources_names)
{
if (type == POLYGON) {
cm_->declare_parameter(
Expand Down Expand Up @@ -408,6 +410,13 @@ void Tester::addPolygon(
polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name));
cm_->set_parameter(
rclcpp::Parameter(polygon_name + ".polygon_pub_topic", polygon_name));

if (!sources_names.empty()) {
cm_->declare_parameter(
polygon_name + ".sources_names", rclcpp::ParameterValue(sources_names));
cm_->set_parameter(
rclcpp::Parameter(polygon_name + ".sources_names", sources_names));
}
}

void Tester::addPolygonVelocitySubPolygon(
Expand Down Expand Up @@ -1499,10 +1508,11 @@ TEST_F(Tester, testCollisionPointsMarkers)
// Share TF
sendTransforms(curr_time);

// If no source published, no marker
publishCmdVel(0.5, 0.2, 0.1);
ASSERT_TRUE(waitCollisionPointsMarker(500ms));
ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u);
ASSERT_FALSE(waitCollisionPointsMarker(500ms));

// Publish source and check markers are published
publishScan(0.5, curr_time);
ASSERT_TRUE(waitData(0.5, 500ms, curr_time));
publishCmdVel(0.5, 0.2, 0.1);
Expand Down Expand Up @@ -1580,6 +1590,48 @@ TEST_F(Tester, testVelocityPolygonStop)
cm_->stop();
}

TEST_F(Tester, testSourceAssociatedToPolygon)
{
// Set Collision Monitor parameters:
// - 2 sources (scan and range)
// - 1 stop polygon associated to range source
// - 1 slowdown polygon (associated with all sources by default)
setCommonParameters();
addSource(SCAN_NAME, SCAN);
addSource(RANGE_NAME, RANGE);
std::vector<std::string> range_only_sources_names = {RANGE_NAME};
std::vector<std::string> all_sources_names = {SCAN_NAME, RANGE_NAME};
addPolygon("StopOnRangeSource", POLYGON, 1.0, "stop", range_only_sources_names);
addPolygon("SlowdownOnAllSources", POLYGON, 1.0, "slowdown");
setVectors({"StopOnRangeSource", "SlowdownOnAllSources"}, {SCAN_NAME, RANGE_NAME});

// Start Collision Monitor node
cm_->start();

// Share TF
rclcpp::Time curr_time = cm_->now();
sendTransforms(curr_time);

// Publish sources so that :
// - scan obstacle is in polygons
// - range obstacle is far away from polygons
publishScan(0.5, curr_time);
publishRange(4.5, curr_time);
ASSERT_TRUE(waitData(0.5, 500ms, curr_time));

// Publish cmd vel
publishCmdVel(0.5, 0.0, 0.0);
ASSERT_TRUE(waitCmdVel(500ms));

// Since the stop polygon is only checking range source, slowdown action should be applied
ASSERT_TRUE(waitActionState(500ms));
ASSERT_EQ(action_state_->action_type, SLOWDOWN);
ASSERT_EQ(action_state_->polygon_name, "SlowdownOnAllSources");

// Stop Collision Monitor node
cm_->stop();
}

int main(int argc, char ** argv)
{
// Initialize the system
Expand Down
87 changes: 79 additions & 8 deletions nav2_collision_monitor/test/polygons_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@ static const char POLYGON_SUB_TOPIC[]{"polygon_sub"};
static const char POLYGON_PUB_TOPIC[]{"polygon_pub"};
static const char POLYGON_NAME[]{"TestPolygon"};
static const char CIRCLE_NAME[]{"TestCircle"};
static const char OBSERVATION_SOURCE_NAME[]{"source"};
static const std::vector<double> SQUARE_POLYGON {
0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5};
static const std::vector<double> ARBITRARY_POLYGON {
Expand Down Expand Up @@ -235,7 +236,9 @@ class Tester : public ::testing::Test

protected:
// Working with parameters
void setCommonParameters(const std::string & polygon_name, const std::string & action_type);
void setCommonParameters(
const std::string & polygon_name, const std::string & action_type,
const std::vector<std::string> & sources_names = std::vector<std::string>());
void setPolygonParameters(
const char * points,
const bool is_static);
Expand Down Expand Up @@ -289,7 +292,9 @@ Tester::~Tester()
tf_buffer_.reset();
}

void Tester::setCommonParameters(const std::string & polygon_name, const std::string & action_type)
void Tester::setCommonParameters(
const std::string & polygon_name, const std::string & action_type,
const std::vector<std::string> & sources_names)
{
test_node_->declare_parameter(
polygon_name + ".action_type", rclcpp::ParameterValue(action_type));
Expand Down Expand Up @@ -336,6 +341,21 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st
polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(POLYGON_PUB_TOPIC));
test_node_->set_parameter(
rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC));

if (!test_node_->has_parameter("observation_sources")) {
std::vector<std::string> default_observation_sources = {OBSERVATION_SOURCE_NAME};
test_node_->declare_parameter(
"observation_sources", rclcpp::ParameterValue(default_observation_sources));
test_node_->set_parameter(
rclcpp::Parameter("observation_sources", default_observation_sources));
}

if (!sources_names.empty()) {
test_node_->declare_parameter(
polygon_name + ".sources_names", rclcpp::ParameterValue(sources_names));
test_node_->set_parameter(
rclcpp::Parameter(polygon_name + ".sources_names", sources_names));
}
}

void Tester::setPolygonParameters(
Expand Down Expand Up @@ -863,23 +883,23 @@ TEST_F(Tester, testPolygonGetCollisionTime)
nav2_collision_monitor::Velocity vel{0.5, 0.0, 0.0}; // 0.5 m/s forward movement
// Two points 0.2 m ahead the footprint (0.5 m)
std::unordered_map<std::string, std::vector<nav2_collision_monitor::Point>> points_map;
points_map.insert({"source", {{0.7, -0.01}, {0.7, 0.01}}});
points_map.insert({OBSERVATION_SOURCE_NAME, {{0.7, -0.01}, {0.7, 0.01}}});
// Collision is expected to be ~= 0.2 m / 0.5 m/s seconds
EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.4, SIMULATION_TIME_STEP);

// Backward movement check
vel = {-0.5, 0.0, 0.0}; // 0.5 m/s backward movement
// Two points 0.2 m behind the footprint (0.5 m)
points_map.clear();
points_map.insert({"source", {{-0.7, -0.01}, {-0.7, 0.01}}});
points_map.insert({OBSERVATION_SOURCE_NAME, {{-0.7, -0.01}, {-0.7, 0.01}}});
// Collision is expected to be in ~= 0.2 m / 0.5 m/s seconds
EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.4, SIMULATION_TIME_STEP);

// Sideway movement check
vel = {0.0, 0.5, 0.0}; // 0.5 m/s sideway movement
// Two points 0.1 m ahead the footprint (0.5 m)
points_map.clear();
points_map.insert({"source", {{-0.01, 0.6}, {0.01, 0.6}}});
points_map.insert({OBSERVATION_SOURCE_NAME, {{-0.01, 0.6}, {0.01, 0.6}}});
// Collision is expected to be in ~= 0.1 m / 0.5 m/s seconds
EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.2, SIMULATION_TIME_STEP);

Expand All @@ -896,7 +916,7 @@ TEST_F(Tester, testPolygonGetCollisionTime)
// -----------
// '
points_map.clear();
points_map.insert({"source", {{0.49, -0.01}, {0.49, 0.01}}});
points_map.insert({OBSERVATION_SOURCE_NAME, {{0.49, -0.01}, {0.49, 0.01}}});
// Collision is expected to be in ~= 45 degrees * M_PI / (180 degrees * 1.0 rad/s) seconds
double exp_res = 45 / 180 * M_PI;
EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), exp_res, EPSILON);
Expand All @@ -905,15 +925,15 @@ TEST_F(Tester, testPolygonGetCollisionTime)
vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement
// Two points inside
points_map.clear();
points_map.insert({"source", {{0.1, -0.01}, {0.1, 0.01}}});
points_map.insert({OBSERVATION_SOURCE_NAME, {{0.1, -0.01}, {0.1, 0.01}}});
// Collision already appeared: collision time should be 0
EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.0, EPSILON);

// All points are out of simulation prediction
vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement
// Two points 0.6 m ahead the footprint (0.5 m)
points_map.clear();
points_map.insert({"source", {{1.1, -0.01}, {1.1, 0.01}}});
points_map.insert({OBSERVATION_SOURCE_NAME, {{1.1, -0.01}, {1.1, 0.01}}});
// There is no collision: return value should be negative
EXPECT_LT(polygon_->getCollisionTime(points_map, vel), 0.0);
}
Expand Down Expand Up @@ -978,6 +998,57 @@ TEST_F(Tester, testPolygonInvalidPointsString)
ASSERT_FALSE(polygon_->configure());
}

TEST_F(Tester, testPolygonSourceDefaultAssociation)
{
std::vector<std::string> all_sources = {"source_1", "source_2", "source_3"};
test_node_->declare_parameter(
"observation_sources", rclcpp::ParameterValue(all_sources));
test_node_->set_parameter(
rclcpp::Parameter("observation_sources", all_sources));

setCommonParameters(POLYGON_NAME, "stop");
setPolygonParameters(SQUARE_POLYGON_STR, true);
polygon_ = std::make_shared<PolygonWrapper>(
test_node_, POLYGON_NAME,
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
polygon_->configure();
ASSERT_EQ(polygon_->getSourcesNames(), all_sources);
}

TEST_F(Tester, testPolygonSourceInvalidAssociation)
{
std::vector<std::string> all_sources = {"source_1", "source_2", "source_3"};
test_node_->declare_parameter(
"observation_sources", rclcpp::ParameterValue(all_sources));
test_node_->set_parameter(
rclcpp::Parameter("observation_sources", all_sources));

setCommonParameters(POLYGON_NAME, "stop", std::vector<std::string>({"source_1", "source_4"}));
setPolygonParameters(SQUARE_POLYGON_STR, true);
polygon_ = std::make_shared<PolygonWrapper>(
test_node_, POLYGON_NAME,
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
ASSERT_FALSE(polygon_->configure());
}

TEST_F(Tester, testPolygonSourceAssociation)
{
std::vector<std::string> all_sources = {"source_1", "source_2", "source_3"};
std::vector<std::string> partial_sources = {"source_1", "source_3"};
test_node_->declare_parameter(
"observation_sources", rclcpp::ParameterValue(all_sources));
test_node_->set_parameter(
rclcpp::Parameter("observation_sources", all_sources));

setCommonParameters(POLYGON_NAME, "stop", partial_sources);
setPolygonParameters(SQUARE_POLYGON_STR, true);
polygon_ = std::make_shared<PolygonWrapper>(
test_node_, POLYGON_NAME,
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
polygon_->configure();
ASSERT_EQ(polygon_->getSourcesNames(), partial_sources);
}

int main(int argc, char ** argv)
{
// Initialize the system
Expand Down

0 comments on commit e53372f

Please sign in to comment.