Skip to content

Commit

Permalink
[collision monitor] Add temporal axis to polygon detection (ros-navig…
Browse files Browse the repository at this point in the history
…ation#4364)

The polygon action triggers when `min_points` from the detection sources
are inside the polygon under test. In case of noisy measurement close to
the threshold, this could cause jerks on the robot velocity command.

* Add polygon parameters `min_consecutive_in` / `min_consecutive_out`
  that defines the consecutive number of times the controller loop must
  detect `min_points` inside / outside the given polygon to activate its
  action. Those parameters act as a Schmitt trigger low and high
  threshold.

Signed-off-by: Antoine Gennart <gennartan@disroot.org>
  • Loading branch information
gennartan committed Jun 1, 2024
1 parent d302f1a commit b1c5d88
Show file tree
Hide file tree
Showing 5 changed files with 125 additions and 6 deletions.
23 changes: 23 additions & 0 deletions nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -145,6 +145,19 @@ class Polygon
*/
virtual int getPointsInside(const std::vector<Point> & points) const;

/**
* @brief Add a new decision point to count the consecutive number of
* time enough point are in the polygon. Updates the current decision value
* based on previous measurements.
*/
void newDecisionPoint(bool is_in);

/**
* @brief Get current polygon decision
* @return True if the polygon action must be applied, false otherwise
*/
bool getDecision() const;

/**
* @brief Obtains estimated (simulated) time before a collision.
* Applicable for APPROACH model.
Expand Down Expand Up @@ -251,6 +264,10 @@ class Polygon
ActionType action_type_;
/// @brief Minimum number of data readings within a zone to trigger the action
int min_points_;
/// @brief Minimum number of consecutive data with minimum min_points to trigger the action
int min_consecutive_in_;
/// @brief Minimum number of consecutive data without minimum min_points to stop the action
int min_consecutive_out_;
/// @brief Robot slowdown (share of its actual speed)
double slowdown_ratio_;
/// @brief Robot linear limit
Expand All @@ -277,6 +294,12 @@ class Polygon
std::string base_frame_id_;
/// @brief Transform tolerance
tf2::Duration transform_tolerance_;
/// @brief Current decision for the polygon (apply or not apply the action)
bool current_decision_;
/// @brief Current number of consecutive data in to trigger the action
int current_consecutive_in_;
/// @brief Current number of consecutive data out to trigger the action
int current_consecutive_out_;

// Visualization
/// @brief Whether to publish the polygon
Expand Down
8 changes: 5 additions & 3 deletions nav2_collision_monitor/src/collision_detector_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -370,9 +370,11 @@ void CollisionDetector::process()
continue;
}
state_msg->polygons.push_back(polygon->getName());
state_msg->detections.push_back(
polygon->getPointsInside(
collision_points) >= polygon->getMinPoints());
polygon->newDecisionPoint(
polygon->getPointsInside(collision_points) >=
polygon->getMinPoints());

state_msg->detections.push_back(polygon->getDecision());
}

state_pub_->publish(std::move(state_msg));
Expand Down
7 changes: 5 additions & 2 deletions nav2_collision_monitor/src/collision_monitor_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -514,7 +514,9 @@ bool CollisionMonitor::processStopSlowdownLimit(
return false;
}

if (polygon->getPointsInside(collision_points) >= polygon->getMinPoints()) {
polygon->newDecisionPoint(polygon->getPointsInside(collision_points) >= polygon->getMinPoints());

if (polygon->getDecision()) {
if (polygon->getActionType() == STOP) {
// Setting up zero velocity for STOP model
robot_action.polygon_name = polygon->getName();
Expand Down Expand Up @@ -571,7 +573,8 @@ bool CollisionMonitor::processApproach(

// Obtain time before a collision
const double collision_time = polygon->getCollisionTime(collision_points, velocity);
if (collision_time >= 0.0) {
polygon->newDecisionPoint(collision_time >= 0.0);
if (polygon->getDecision() && collision_time >= 0.0) {
// If collision will occurr, reduce robot speed
const double change_ratio = collision_time / polygon->getTimeBeforeCollision();
const Velocity safe_vel = velocity * change_ratio;
Expand Down
33 changes: 32 additions & 1 deletion nav2_collision_monitor/src/polygon.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,8 @@ Polygon::Polygon(
: node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING),
slowdown_ratio_(0.0), linear_limit_(0.0), angular_limit_(0.0),
footprint_sub_(nullptr), tf_buffer_(tf_buffer),
base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance)
base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance),
current_decision_(false), current_consecutive_in_(0), current_consecutive_out_(0)
{
RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str());
}
Expand Down Expand Up @@ -229,6 +230,28 @@ int Polygon::getPointsInside(const std::vector<Point> & points) const
return num;
}

void Polygon::newDecisionPoint(bool is_in)
{
if (is_in && current_consecutive_in_ < min_consecutive_in_) {
current_consecutive_in_++;
current_consecutive_out_ = 0;
if (current_consecutive_in_ >= min_consecutive_in_) {
current_decision_ = true;
}
} else if (!is_in && current_consecutive_out_ < min_consecutive_out_) {
current_consecutive_in_ = 0;
current_consecutive_out_++;
if (current_consecutive_out_ >= min_consecutive_out_) {
current_decision_ = false;
}
}
}

bool Polygon::getDecision() const
{
return current_decision_;
}

double Polygon::getCollisionTime(
const std::vector<Point> & collision_points,
const Velocity & velocity) const
Expand Down Expand Up @@ -322,6 +345,14 @@ bool Polygon::getCommonParameters(
node, polygon_name_ + ".min_points", rclcpp::ParameterValue(4));
min_points_ = node->get_parameter(polygon_name_ + ".min_points").as_int();

nav2_util::declare_parameter_if_not_declared(
node, polygon_name_ + ".min_consecutive_in", rclcpp::ParameterValue(1));
min_consecutive_in_ = node->get_parameter(polygon_name_ + ".min_consecutive_in").as_int();

nav2_util::declare_parameter_if_not_declared(
node, polygon_name_ + ".min_consecutive_out", rclcpp::ParameterValue(1));
min_consecutive_out_ = node->get_parameter(polygon_name_ + ".min_consecutive_out").as_int();

try {
nav2_util::declare_parameter_if_not_declared(
node, polygon_name_ + ".max_points", rclcpp::PARAMETER_INTEGER);
Expand Down
60 changes: 60 additions & 0 deletions nav2_collision_monitor/test/polygons_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,8 @@ static const char INVALID_POINTS_STR[]{
};
static const double CIRCLE_RADIUS{0.5};
static const int MIN_POINTS{2};
static const int MIN_CONSECUTIVE_IN{1};
static const int MIN_CONSECUTIVE_OUT{1};
static const double SLOWDOWN_RATIO{0.7};
static const double LINEAR_LIMIT{0.4};
static const double ANGULAR_LIMIT{0.09};
Expand Down Expand Up @@ -336,6 +338,17 @@ 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));

test_node_->declare_parameter(
polygon_name + ".min_consecutive_in", rclcpp::ParameterValue(MIN_CONSECUTIVE_IN));
test_node_->set_parameter(
rclcpp::Parameter(polygon_name + ".min_consecutive_in", MIN_CONSECUTIVE_IN));

test_node_->declare_parameter(
polygon_name + ".min_consecutive_out", rclcpp::ParameterValue(MIN_CONSECUTIVE_OUT));
test_node_->set_parameter(
rclcpp::Parameter(polygon_name + ".min_consecutive_out", MIN_CONSECUTIVE_OUT));

}

void Tester::setPolygonParameters(
Expand Down Expand Up @@ -977,6 +990,53 @@ TEST_F(Tester, testPolygonInvalidPointsString)
ASSERT_FALSE(polygon_->configure());
}


TEST_F(Tester, testPolygonMinConsecutiveParam)
{
createPolygon("stop", true);

// Test polygon with default parameters
ASSERT_FALSE(polygon_->getDecision());
polygon_->newDecisionPoint(false);
ASSERT_FALSE(polygon_->getDecision());
polygon_->newDecisionPoint(true);
ASSERT_TRUE(polygon_->getDecision());
polygon_->newDecisionPoint(false);
ASSERT_FALSE(polygon_->getDecision());

// Create new polygon with non default parameter
test_node_->set_parameter(
rclcpp::Parameter(std::string(POLYGON_NAME) + ".min_consecutive_in", 3));
test_node_->set_parameter(
rclcpp::Parameter(std::string(POLYGON_NAME) + ".min_consecutive_out", 2));

polygon_ = std::make_shared<PolygonWrapper>(
test_node_, POLYGON_NAME,
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
ASSERT_TRUE(polygon_->configure());
polygon_->activate();

// Require 3 consecutive in to get decistion to true
// Require 2 consecutive out to get decistion to false
ASSERT_FALSE(polygon_->getDecision());
polygon_->newDecisionPoint(false);
ASSERT_FALSE(polygon_->getDecision());
polygon_->newDecisionPoint(true);
ASSERT_FALSE(polygon_->getDecision());
polygon_->newDecisionPoint(true);
ASSERT_FALSE(polygon_->getDecision());
polygon_->newDecisionPoint(true);
ASSERT_TRUE(polygon_->getDecision());
polygon_->newDecisionPoint(false);
ASSERT_TRUE(polygon_->getDecision());
polygon_->newDecisionPoint(true);
ASSERT_TRUE(polygon_->getDecision());
polygon_->newDecisionPoint(false);
ASSERT_TRUE(polygon_->getDecision());
polygon_->newDecisionPoint(false);
ASSERT_FALSE(polygon_->getDecision());
}

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

0 comments on commit b1c5d88

Please sign in to comment.