Skip to content

Commit

Permalink
Add support of absolute limits to Speed Filter (#2149)
Browse files Browse the repository at this point in the history
* Add support of absolute limits to Speed Filter

* Fix review items

* Fix comment

* Support for nav2_regulated_pure_pursuit_controller
  • Loading branch information
AlexeyMerzlyakov authored Mar 1, 2021
1 parent 8899b66 commit 992deb0
Show file tree
Hide file tree
Showing 16 changed files with 182 additions and 74 deletions.
2 changes: 1 addition & 1 deletion doc/parameters/param_list.md
Original file line number Diff line number Diff line change
Expand Up @@ -474,7 +474,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame

| Parameter | Default | Description |
| ----------| --------| ------------|
| type | 0 | Type of costmap filter used. This is an enum for the type of filter this should be interpreted as. We provide the following pre-defined types: 0 - keepout zones / preferred lanes filter; 1 - speed filter, speed limit is specified in % of maximum speed; 2 - speed filter, speed limit is specified in absolute value (not implemented yet) |
| type | 0 | Type of costmap filter used. This is an enum for the type of filter this should be interpreted as. We provide the following pre-defined types: 0 - keepout zones / preferred lanes filter; 1 - speed filter, speed limit is specified in % of maximum speed; 2 - speed filter, speed limit is specified in absolute value (m/s) |
| filter_info_topic | "costmap_filter_info" | Topic to publish costmap filter information to |
| mask_topic | "filter_mask" | Topic to publish filter mask to. The value of this parameter should be in accordance with `topic_name` parameter of `map_server` tuned to filter mask publishing |
| base | 0.0 | Base of `OccupancyGrid` mask value -> filter space value linear conversion which is being proceeded as: `filter_space_value = base + multiplier * mask_value` |
Expand Down
6 changes: 1 addition & 5 deletions nav2_controller/src/nav2_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -467,11 +467,7 @@ void ControllerServer::speedLimitCallback(const nav2_msgs::msg::SpeedLimit::Shar
{
ControllerMap::iterator it;
for (it = controllers_.begin(); it != controllers_.end(); ++it) {
if (!msg->percentage) {
RCLCPP_ERROR(get_logger(), "Speed limit in absolute values is not implemented yet");
return;
}
it->second->setSpeedLimit(msg->speed_limit);
it->second->setSpeedLimit(msg->speed_limit, msg->percentage);
}
}

Expand Down
7 changes: 5 additions & 2 deletions nav2_core/include/nav2_core/controller.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -115,9 +115,12 @@ class Controller

/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in percentage from maximum robot speed.
* @param speed_limit expressed in absolute value (in m/s)
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
virtual void setSpeedLimit(const double & speed_limit) = 0;
virtual void setSpeedLimit(const double & speed_limit, const bool & percentage) = 0;
};

} // namespace nav2_core
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,7 @@ class SpeedFilter : public CostmapFilter
std::string global_frame_; // Frame of currnet layer (master_grid)

double base_, multiplier_;
bool percentage_;
double speed_limit_, speed_limit_prev_;
};

Expand Down
51 changes: 28 additions & 23 deletions nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,7 @@ void SpeedFilter::initializeFilter(
// Reset speed conversion states
base_ = BASE_DEFAULT;
multiplier_ = MULTIPLIER_DEFAULT;
percentage_ = false;
}

void SpeedFilter::filterInfoCallback(
Expand Down Expand Up @@ -120,15 +121,19 @@ void SpeedFilter::filterInfoCallback(
multiplier_ = msg->multiplier;
if (msg->type == SPEED_FILTER_PERCENT) {
// Using speed limit in % of maximum speed
percentage_ = true;
RCLCPP_INFO(
logger_,
"SpeedFilter: Using expressed in a percent from maximum speed"
"speed_limit = %f + filter_mask_data * %f",
base_, multiplier_);
} else if (msg->type == SPEED_FILTER_ABSOLUTE) {
// Speed limit in absolute values is not implemented yet
RCLCPP_ERROR(logger_, "SpeedFilter: Speed limit in absolute values is not implemented yet");
return;
// Using speed limit in m/s
percentage_ = false;
RCLCPP_INFO(
logger_,
"SpeedFilter: Using absolute speed_limit = %f + filter_mask_data * %f",
base_, multiplier_);
} else {
RCLCPP_ERROR(logger_, "SpeedFilter: Mode is not supported");
return;
Expand Down Expand Up @@ -278,30 +283,30 @@ void SpeedFilter::process(
} else {
// Normal case: speed_mask_data in range of [1..100]
speed_limit_ = speed_mask_data * multiplier_ + base_;
if (speed_limit_ < 0.0) {
RCLCPP_WARN(
logger_,
"SpeedFilter: Speed limit in filter_mask[%i, %i] is less than 0%%, "
"which can not be true. Setting it to 0%% value.",
mask_robot_i, mask_robot_j);
speed_limit_ = NO_SPEED_LIMIT;
}
if (speed_limit_ > 100.0) {
RCLCPP_WARN(
logger_,
"SpeedFilter: Speed limit in filter_mask[%i, %i] is higher than 100%%, "
"which can not be true. Setting it to 100%% value.",
mask_robot_i, mask_robot_j);
speed_limit_ = 100.0;
if (percentage_) {
if (speed_limit_ < 0.0 || speed_limit_ > 100.0) {
RCLCPP_WARN(
logger_,
"SpeedFilter: Speed limit in filter_mask[%i, %i] is %f%%, "
"out of bounds of [0, 100]. Setting it to no-limit value.",
mask_robot_i, mask_robot_j, speed_limit_);
speed_limit_ = NO_SPEED_LIMIT;
}
} else {
if (speed_limit_ < 0.0) {
RCLCPP_WARN(
logger_,
"SpeedFilter: Speed limit in filter_mask[%i, %i] is less than 0 m/s, "
"which can not be true. Setting it to no-limit value.",
mask_robot_i, mask_robot_j);
speed_limit_ = NO_SPEED_LIMIT;
}
}
}

if (speed_limit_ != speed_limit_prev_) {
if (speed_limit_ != NO_SPEED_LIMIT) {
RCLCPP_DEBUG(
logger_,
"SpeedFilter: Speed limit is set to %f%% of maximum speed",
speed_limit_);
RCLCPP_DEBUG(logger_, "SpeedFilter: Speed limit is set to %f", speed_limit_);
} else {
RCLCPP_DEBUG(logger_, "SpeedFilter: Speed limit is set to its default value");
}
Expand All @@ -311,7 +316,7 @@ void SpeedFilter::process(
std::make_unique<nav2_msgs::msg::SpeedLimit>();
msg->header.frame_id = global_frame_;
msg->header.stamp = clock_->now();
msg->percentage = true;
msg->percentage = percentage_;
msg->speed_limit = speed_limit_;
speed_limit_pub_->publish(std::move(msg));

Expand Down
58 changes: 40 additions & 18 deletions nav2_costmap_2d/test/unit/speed_filter_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -223,7 +223,7 @@ class TestNode : public ::testing::Test
uint8_t type, double base, double multiplier,
double tr_x, double tr_y);
void testOutOfMask(uint8_t type, double base, double multiplier);
void testIncorrectPercentage(uint8_t type, double base, double multiplier);
void testIncorrectLimits(uint8_t type, double base, double multiplier);

void reset();

Expand Down Expand Up @@ -420,16 +420,20 @@ void TestNode::verifySpeedLimit(
// expected_limit is being calculated by using float32 base and multiplier
double expected_limit = cost * multiplier + base;
if (type == nav2_costmap_2d::SPEED_FILTER_PERCENT) {
if (expected_limit < 0.0) {
if (expected_limit < 0.0 || expected_limit > 100.0) {
expected_limit = nav2_costmap_2d::NO_SPEED_LIMIT;
}
if (expected_limit > 100.0) {
expected_limit = 100.0;
}
EXPECT_TRUE(speed_limit->percentage);
EXPECT_TRUE(speed_limit->speed_limit >= 0.0);
EXPECT_TRUE(speed_limit->speed_limit <= 100.0);
EXPECT_NEAR(speed_limit->speed_limit, expected_limit, EPSILON);
} else if (type == nav2_costmap_2d::SPEED_FILTER_ABSOLUTE) {
if (expected_limit < 0.0) {
expected_limit = nav2_costmap_2d::NO_SPEED_LIMIT;
}
EXPECT_FALSE(speed_limit->percentage);
EXPECT_TRUE(speed_limit->speed_limit >= 0.0);
EXPECT_NEAR(speed_limit->speed_limit, expected_limit, EPSILON);
} else {
FAIL() << "The type of costmap filter is unknown";
}
Expand Down Expand Up @@ -581,7 +585,7 @@ void TestNode::testOutOfMask(uint8_t type, double base, double multiplier)
ASSERT_TRUE(speed_limit == old_speed_limit);
}

void TestNode::testIncorrectPercentage(uint8_t type, double base, double multiplier)
void TestNode::testIncorrectLimits(uint8_t type, double base, double multiplier)
{
const int min_i = 0;
const int min_j = 0;
Expand All @@ -593,14 +597,14 @@ void TestNode::testIncorrectPercentage(uint8_t type, double base, double multipl

std::vector<std::tuple<unsigned int, unsigned int>> points;

// Some middle point corresponding to correct percentage
// Some middle point corresponding to correct speed limit value
points.push_back(std::make_tuple(width_ / 2 - 1, height_ / 2 - 1));
// (0, 1) point corresponding to incorrect percentage: data = 1, percentage < 0%
// (0, 1) point corresponding to incorrect limit value: data = 1, value < 0
points.push_back(std::make_tuple(0, 1));
// Some middle point corresponding to correct percentage
// Some middle point corresponding to correct speed limit value
points.push_back(std::make_tuple(width_ / 2 - 1, height_ / 2 - 1));
// (width_ - 1, height_ - 1) point corresponding to incorrect percentage:
// data = 100, percentage > 100%
// (width_ - 1, height_ - 1) point corresponding to incorrect limit value:
// data = 100, value > 100
points.push_back(std::make_tuple(width_ - 1, height_ - 1));

for (auto it = points.begin(); it != points.end(); ++it) {
Expand Down Expand Up @@ -650,7 +654,7 @@ TEST_F(TestNode, testIncorrectPercentSpeedLimit)
EXPECT_TRUE(createSpeedFilter("map"));

// Test SpeedFilter
testIncorrectPercentage(nav2_costmap_2d::SPEED_FILTER_PERCENT, -50.0, 2.0);
testIncorrectLimits(nav2_costmap_2d::SPEED_FILTER_PERCENT, -50.0, 2.0);

// Clean-up
speed_filter_->resetFilter();
Expand All @@ -661,8 +665,26 @@ TEST_F(TestNode, testAbsoluteSpeedLimit)
{
// Initilize test system
createMaps("map");
publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -1.23, 4.5);
EXPECT_FALSE(createSpeedFilter("map"));
publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5);
EXPECT_TRUE(createSpeedFilter("map"));

// Test SpeedFilter
testFullMask(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5, NO_TRANSLATION, NO_TRANSLATION);

// Clean-up
speed_filter_->resetFilter();
reset();
}

TEST_F(TestNode, testIncorrectAbsoluteSpeedLimit)
{
// Initilize test system
createMaps("map");
publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -50.0, 2.0);
EXPECT_TRUE(createSpeedFilter("map"));

// Test SpeedFilter
testIncorrectLimits(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -50.0, 2.0);

// Clean-up
speed_filter_->resetFilter();
Expand All @@ -688,7 +710,7 @@ TEST_F(TestNode, testInfoRePublish)
{
// Initilize test system
createMaps("map");
publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 1.2, 3.4);
publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5);
EXPECT_TRUE(createSpeedFilter("map"));

// Re-publish filter info (with incorrect base and multiplier)
Expand All @@ -708,15 +730,15 @@ TEST_F(TestNode, testMaskRePublish)
{
// Initilize test system
createMaps("map");
publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.1, 0.2);
publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5);
EXPECT_TRUE(createSpeedFilter("map"));

// Re-publish filter mask and test that everything is working after
rePublishMask();

// Test SpeedFilter
testSimpleMask(
nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.1, 0.2, NO_TRANSLATION, NO_TRANSLATION);
nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5, NO_TRANSLATION, NO_TRANSLATION);

// Clean-up
speed_filter_->resetFilter();
Expand All @@ -727,7 +749,7 @@ TEST_F(TestNode, testIncorrectFilterType)
{
// Initilize test system
createMaps("map");
publishMaps(INCORRECT_TYPE, -1.23, 4.5);
publishMaps(INCORRECT_TYPE, 1.23, 4.5);
EXPECT_FALSE(createSpeedFilter("map"));

// Clean-up
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -133,12 +133,15 @@ class DWBLocalPlanner : public nav2_core::Controller

/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in percentage from maximum robot speed.
* @param speed_limit expressed in absolute value (in m/s)
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
void setSpeedLimit(const double & speed_limit) override
void setSpeedLimit(const double & speed_limit, const bool & percentage) override
{
if (traj_generator_) {
traj_generator_->setSpeedLimit(speed_limit);
traj_generator_->setSpeedLimit(speed_limit, percentage);
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -126,9 +126,12 @@ class TrajectoryGenerator

/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in percentage from maximum robot speed.
* @param speed_limit expressed in absolute value (in m/s)
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
virtual void setSpeedLimit(const double & speed_limit) = 0;
virtual void setSpeedLimit(const double & speed_limit, const bool & percentage) = 0;
};

} // namespace dwb_core
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ class KinematicsHandler

inline KinematicParameters getKinematics() {return *kinematics_.load();}

void setSpeedLimit(const double & speed_limit);
void setSpeedLimit(const double & speed_limit, const bool & percentage);

using Ptr = std::shared_ptr<KinematicsHandler>;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -70,12 +70,15 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator

/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in percentage from maximum robot speed.
* @param speed_limit expressed in absolute value (in m/s)
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
void setSpeedLimit(const double & speed_limit) override
void setSpeedLimit(const double & speed_limit, const bool & percentage) override
{
if (kinematics_handler_) {
kinematics_handler_->setSpeedLimit(speed_limit);
kinematics_handler_->setSpeedLimit(speed_limit, percentage);
}
}

Expand Down
28 changes: 22 additions & 6 deletions nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -129,7 +129,8 @@ void KinematicsHandler::initialize(
update_kinematics(kinematics);
}

void KinematicsHandler::setSpeedLimit(const double & speed_limit)
void KinematicsHandler::setSpeedLimit(
const double & speed_limit, const bool & percentage)
{
KinematicParameters kinematics(*kinematics_.load());

Expand All @@ -140,11 +141,26 @@ void KinematicsHandler::setSpeedLimit(const double & speed_limit)
kinematics.max_vel_y_ = kinematics.base_max_vel_y_;
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_;
} else {
// Speed limit is expressed in % from maximum speed of robot
kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_ * speed_limit / 100.0;
kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * speed_limit / 100.0;
kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * speed_limit / 100.0;
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * speed_limit / 100.0;
if (percentage) {
// Speed limit is expressed in % from maximum speed of robot
kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_ * speed_limit / 100.0;
kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * speed_limit / 100.0;
kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * speed_limit / 100.0;
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * speed_limit / 100.0;
} else {
// Speed limit is expressed in absolute value
if (speed_limit < kinematics.base_max_speed_xy_) {
kinematics.max_speed_xy_ = speed_limit;
// Handling components and angular velocity changes:
// Max velocities are being changed in the same proportion
// as absolute linear speed changed in order to preserve
// robot moving trajectories to be the same after speed change.
const double ratio = speed_limit / kinematics.base_max_speed_xy_;
kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * ratio;
kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * ratio;
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * ratio;
}
}
}

// Do not forget to update max_speed_xy_sq_ as well
Expand Down
Loading

0 comments on commit 992deb0

Please sign in to comment.