Skip to content

Commit

Permalink
add test on circle radius update
Browse files Browse the repository at this point in the history 
Signed-off-by: asarazin <[email protected]>
  • Loading branch information
asarazin committed Apr 11, 2024
1 parent 49e3eb8 commit aab2dfa
Show file tree
Hide file tree
Showing 5 changed files with 118 additions and 65 deletions.
7 changes: 4 additions & 3 deletions nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,9 +69,10 @@ class Circle : public Polygon
int getPointsInside(const std::vector<Point> & points) const override;

/**
* @brief Specifies that the shape is always set for a circle object
* @brief Returns true if circle radius is set.
* Otherwise, prints a warning and returns false.
*/
bool isShapeSet() override {return true;}
bool isShapeSet() override;

protected:
/**
Expand Down Expand Up @@ -112,7 +113,7 @@ class Circle : public Polygon
/// @brief Radius of the circle
double radius_;
/// @brief (radius * radius) value. Stored for optimization.
double radius_squared_;
double radius_squared_ = -1.0;
/// @brief Radius subscription
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr radius_sub_;
}; // class Circle
Expand Down
2 changes: 1 addition & 1 deletion nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,7 @@ class Polygon

/**
* @brief Returns true if polygon points were set.
* Othewise, prints a warning and returns false.
* Otherwise, prints a warning and returns false.
*/
virtual bool isShapeSet();

Expand Down
43 changes: 24 additions & 19 deletions nav2_collision_monitor/src/circle.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,15 @@ int Circle::getPointsInside(const std::vector<Point> & points) const
return num;
}

bool Circle::isShapeSet()
{
if (radius_squared_ == -1.0) {
RCLCPP_WARN(logger_, "[%s]: Circle radius is not set yet", polygon_name_.c_str());
return false;
}
return true;
}

bool Circle::getParameters(
std::string & polygon_sub_topic,
std::string & polygon_pub_topic,
Expand All @@ -83,40 +92,36 @@ bool Circle::getParameters(
// Clear the polygon subscription topic. It will be set later, if necessary.
polygon_sub_topic.clear();

if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) {
return false;
}

// There is no footprint subscription for the Circle. Thus, set string as empty.
footprint_topic.clear();

bool radius_defined = false;
try {
// Leave it not initialized: the will cause an error if it will not set
nav2_util::declare_parameter_if_not_declared(
node, polygon_name_ + ".radius", rclcpp::PARAMETER_DOUBLE);
radius_ = node->get_parameter(polygon_name_ + ".radius").as_double();
radius_squared_ = radius_ * radius_;

// Do not need to proceed further, if "radius" parameter is defined.
// Static polygon will be used.
polygon_sub_topic.clear();
return true;
radius_defined = true;
} catch (const rclcpp::exceptions::ParameterUninitializedException &) {
RCLCPP_INFO(
logger_,
"[%s]: Polygon circle radius is not defined. Using dynamic subscription instead.",
polygon_name_.c_str());
}

if (polygon_sub_topic.empty()) {
RCLCPP_ERROR(
logger_,
"[%s]: Error while getting circle parameters: static radius and sub topic both not defined",
polygon_name_.c_str());
return false;
bool ret = true;
if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) {
if (!radius_defined && polygon_sub_topic.empty()) {
RCLCPP_ERROR(
logger_,
"[%s]: Error while getting circle parameters: static radius and sub topic both not defined",
polygon_name_.c_str());
}
ret = false;
}

return true;
// There is no footprint subscription for the Circle. Thus, set string as empty.
footprint_topic.clear();

return ret;
}

void Circle::createSubscription(std::string & polygon_sub_topic)
Expand Down
57 changes: 28 additions & 29 deletions nav2_collision_monitor/src/polygon.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -364,25 +364,27 @@ bool Polygon::getCommonParameters(
polygon_pub_topic = node->get_parameter(polygon_name_ + ".polygon_pub_topic").as_string();
}

if (action_type_ != APPROACH) {
// Get polygon sub topic, in case dynamic polygon will be used
nav2_util::declare_parameter_if_not_declared(
node, polygon_name_ + ".polygon_sub_topic", rclcpp::ParameterValue(""));
polygon_sub_topic =
node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string();
} else {
// Obtain the footprint topic to make a footprint subscription for approach polygon
nav2_util::declare_parameter_if_not_declared(
node, polygon_name_ + ".footprint_topic",
rclcpp::ParameterValue("local_costmap/published_footprint"));
footprint_topic =
node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
}

nav2_util::declare_parameter_if_not_declared(
node, polygon_name_ + ".polygon_subscribe_transient_local", rclcpp::ParameterValue(false));
polygon_subscribe_transient_local_ =
node->get_parameter(polygon_name_ + ".polygon_subscribe_transient_local").as_bool();

if (!isShapeSet()) {
// If shape is not already set, dynamic subscription will be used
if (action_type_ != APPROACH) {
// Get polygon sub topic
nav2_util::declare_parameter_if_not_declared(node, polygon_name_ + ".polygon_sub_topic");
polygon_sub_topic =
node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string();
} else {
// Obtain the footprint topic to make a footprint subscription for approach polygon
nav2_util::declare_parameter_if_not_declared(
node, polygon_name_ + ".footprint_topic",
rclcpp::ParameterValue("local_costmap/published_footprint"));
footprint_topic =
node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
}
}
} catch (const std::exception & ex) {
RCLCPP_ERROR(
logger_,
Expand All @@ -408,37 +410,34 @@ bool Polygon::getParameters(
polygon_sub_topic.clear();
footprint_topic.clear();

if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) {
return false;
}

bool points_defined = false;
try {
// Leave it uninitialized: it will throw an inner exception if the parameter is not set
nav2_util::declare_parameter_if_not_declared(
node, polygon_name_ + ".points", rclcpp::PARAMETER_STRING);
std::string poly_string =
node->get_parameter(polygon_name_ + ".points").as_string();

// Do not need to proceed further, if "points" parameter is defined.
// Static polygon will be used.
polygon_sub_topic.clear();
footprint_topic.clear();
return getPolygonFromString(poly_string, poly_);
points_defined = getPolygonFromString(poly_string, poly_);
} catch (const rclcpp::exceptions::ParameterUninitializedException &) {
RCLCPP_INFO(
logger_,
"[%s]: Polygon points are not defined. Using dynamic subscription instead.",
polygon_name_.c_str());
}

if (polygon_sub_topic.empty() && footprint_topic.empty()) {
RCLCPP_ERROR(
logger_,
"[%s]: Error while getting polygon parameters: static points and sub topic both not defined",
polygon_name_.c_str());
if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) {
if (!points_defined && polygon_sub_topic.empty() && footprint_topic.empty()) {
RCLCPP_ERROR(
logger_,
"[%s]: Error while getting polygon parameters:"
" static points and sub topic both not defined",
polygon_name_.c_str());
}
return false;
}


return true;
}

Expand Down
74 changes: 61 additions & 13 deletions nav2_collision_monitor/test/polygons_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,17 @@ class TestNode : public nav2_util::LifecycleNode
polygon_pub_->publish(std::move(msg));
}

void publishRadius()
{
radius_pub_ = this->create_publisher<std_msgs::msg::Float32>(
POLYGON_SUB_TOPIC, rclcpp::SystemDefaultsQoS());

std::unique_ptr<std_msgs::msg::Float32> msg = std::make_unique<std_msgs::msg::Float32>();
msg->data = CIRCLE_RADIUS;

radius_pub_->publish(std::move(msg));
}

void publishFootprint()
{
footprint_pub_ = this->create_publisher<geometry_msgs::msg::PolygonStamped>(
Expand Down Expand Up @@ -159,6 +170,7 @@ class TestNode : public nav2_util::LifecycleNode

private:
rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_pub_;
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr radius_pub_;
rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_pub_;
rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_sub_;

Expand Down Expand Up @@ -227,11 +239,11 @@ class Tester : public ::testing::Test
void setPolygonParameters(
const char * points,
const bool is_static);
void setCircleParameters(const double radius);
void setCircleParameters(const double radius, const bool is_static);
bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param);
// Creating routines
void createPolygon(const std::string & action_type, const bool is_static);
void createCircle(const std::string & action_type);
void createCircle(const std::string & action_type, const bool is_static);

void sendTransforms(double shift);

Expand All @@ -240,6 +252,9 @@ class Tester : public ::testing::Test
const std::chrono::nanoseconds & timeout,
std::vector<nav2_collision_monitor::Point> & poly);

// Wait until circle polygon radius will be received
bool waitRadius(const std::chrono::nanoseconds & timeout);

// Wait until footprint will be received
bool waitFootprint(
const std::chrono::nanoseconds & timeout,
Expand Down Expand Up @@ -344,12 +359,19 @@ void Tester::setPolygonParameters(
}
}

void Tester::setCircleParameters(const double radius)
void Tester::setCircleParameters(const double radius, const bool is_static)
{
test_node_->declare_parameter(
std::string(CIRCLE_NAME) + ".radius", rclcpp::ParameterValue(radius));
test_node_->set_parameter(
rclcpp::Parameter(std::string(CIRCLE_NAME) + ".radius", radius));
if (is_static) {
test_node_->declare_parameter(
std::string(CIRCLE_NAME) + ".radius", rclcpp::ParameterValue(radius));
test_node_->set_parameter(
rclcpp::Parameter(std::string(CIRCLE_NAME) + ".radius", radius));
} else {
test_node_->declare_parameter(
std::string(CIRCLE_NAME) + ".polygon_sub_topic", rclcpp::ParameterValue(POLYGON_SUB_TOPIC));
test_node_->set_parameter(
rclcpp::Parameter(std::string(CIRCLE_NAME) + ".polygon_sub_topic", POLYGON_SUB_TOPIC));
}
}

bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const std::string & param)
Expand Down Expand Up @@ -382,10 +404,10 @@ void Tester::createPolygon(const std::string & action_type, const bool is_static
polygon_->activate();
}

void Tester::createCircle(const std::string & action_type)
void Tester::createCircle(const std::string & action_type, const bool is_static)
{
setCommonParameters(CIRCLE_NAME, action_type);
setCircleParameters(CIRCLE_RADIUS);
setCircleParameters(CIRCLE_RADIUS, is_static);

circle_ = std::make_shared<CircleWrapper>(
test_node_, CIRCLE_NAME,
Expand Down Expand Up @@ -433,6 +455,19 @@ bool Tester::waitPolygon(
return false;
}

bool Tester::waitRadius(const std::chrono::nanoseconds & timeout)
{
rclcpp::Time start_time = test_node_->now();
while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) {
if (circle_->isShapeSet()) {
return true;
}
rclcpp::spin_some(test_node_->get_node_base_interface());
std::this_thread::sleep_for(10ms);
}
return false;
}

bool Tester::waitFootprint(
const std::chrono::nanoseconds & timeout,
std::vector<nav2_collision_monitor::Point> & footprint)
Expand Down Expand Up @@ -518,7 +553,7 @@ TEST_F(Tester, testPolygonGetApproachParameters)

TEST_F(Tester, testCircleGetParameters)
{
createCircle("approach");
createCircle("approach", true);

// Check that common parameters set correctly
EXPECT_EQ(circle_->getName(), CIRCLE_NAME);
Expand Down Expand Up @@ -628,8 +663,9 @@ TEST_F(Tester, testCircleUndeclaredRadius)
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
ASSERT_FALSE(circle_->configure());

// Check that "radius" parameter is not set after configuring
// Check that "radius" and "polygon_sub_topic" parameters are not set after configuring
ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "radius"));
ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "polygon_sub_topic"));
}

TEST_F(Tester, testPolygonTopicUpdate)
Expand All @@ -646,7 +682,7 @@ TEST_F(Tester, testPolygonTopicUpdate)
ASSERT_FALSE(waitPolygon(100ms, poly));
ASSERT_FALSE(polygon_->isShapeSet());

// Publush correct polygon and make shure that it was set correctly
// Publish correct polygon and make sure that it was set correctly
test_node_->publishPolygon(BASE_FRAME_ID, true);
ASSERT_TRUE(waitPolygon(500ms, poly));
ASSERT_TRUE(polygon_->isShapeSet());
Expand All @@ -661,6 +697,18 @@ TEST_F(Tester, testPolygonTopicUpdate)
EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7], EPSILON);
}

TEST_F(Tester, testCircleTopicUpdate)
{
createCircle("stop", false);
ASSERT_FALSE(circle_->isShapeSet());

// Publish radius and make sure that it was set correctly
test_node_->publishRadius();
ASSERT_TRUE(waitRadius(500ms));
EXPECT_NEAR(circle_->getRadius(), CIRCLE_RADIUS, EPSILON);
EXPECT_NEAR(circle_->getRadiusSquared(), CIRCLE_RADIUS * CIRCLE_RADIUS, EPSILON);
}

TEST_F(Tester, testPolygonTopicUpdateDifferentFrame)
{
createPolygon("stop", false);
Expand Down Expand Up @@ -789,7 +837,7 @@ TEST_F(Tester, testPolygonGetPointsInsideEdge)

TEST_F(Tester, testCircleGetPointsInside)
{
createCircle("stop");
createCircle("stop", true);

std::vector<nav2_collision_monitor::Point> points;
// Point out of radius
Expand Down

0 comments on commit aab2dfa

Please sign in to comment.