From f86e44cedee5b6851e1aa09b20b09554d205b64a Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Fri, 13 Nov 2020 14:51:49 -0800
Subject: [PATCH] Current_ costmap2D updates & fixing recovery-straight-path
issues (#2090)
* WIP attempt at fixing recovery-straight-path issues
* linting!
* handle not current due to old measurement readings separately from resets
---
nav2_controller/src/nav2_controller.cpp | 6 ++
.../nav2_costmap_2d/clear_costmap_service.hpp | 14 +--
.../costmap_filters/costmap_filter.hpp | 1 +
.../include/nav2_costmap_2d/costmap_layer.hpp | 2 +-
.../nav2_costmap_2d/inflation_layer.hpp | 2 +
.../include/nav2_costmap_2d/layer.hpp | 1 +
.../nav2_costmap_2d/obstacle_layer.hpp | 3 +
.../nav2_costmap_2d/range_sensor_layer.hpp | 2 +
.../include/nav2_costmap_2d/static_layer.hpp | 1 +
.../include/nav2_costmap_2d/voxel_layer.hpp | 1 +
.../costmap_filters/costmap_filter.cpp | 2 +
nav2_costmap_2d/plugins/inflation_layer.cpp | 2 +
nav2_costmap_2d/plugins/obstacle_layer.cpp | 10 +-
.../plugins/range_sensor_layer.cpp | 10 +-
nav2_costmap_2d/plugins/static_layer.cpp | 2 +
nav2_costmap_2d/src/clear_costmap_service.cpp | 100 +++---------------
nav2_costmap_2d/src/costmap_layer.cpp | 5 +-
.../test/unit/declare_parameter_test.cpp | 1 +
nav2_msgs/srv/ClearCostmapAroundRobot.srv | 5 +-
nav2_planner/src/planner_server.cpp | 6 ++
20 files changed, 72 insertions(+), 104 deletions(-)
diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp
index ced4dd48..a75f65af 100644
--- a/nav2_controller/src/nav2_controller.cpp
+++ b/nav2_controller/src/nav2_controller.cpp
@@ -297,6 +297,12 @@ void ControllerServer::computeControl()
return;
}
+ // Don't compute a trajectory until costmap is valid (after clear costmap)
+ rclcpp::Rate r(100);
+ while (!costmap_ros_->isCurrent()) {
+ r.sleep();
+ }
+
updateGlobalPath();
computeAndPublishVelocity();
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp
index adee89f1..589f1178 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp
@@ -39,10 +39,7 @@ public:
ClearCostmapService() = delete;
// Clears the region outside of a user-specified area reverting to the static map
- void clearExceptRegion(double reset_distance = 3.0);
-
- // Clears within a window around the robot
- void clearAroundRobot(double window_size_x, double window_size_y);
+ void clearRegion(double reset_distance, bool invert);
// Clears all layers
void clearEntirely();
@@ -77,14 +74,11 @@ private:
const std::shared_ptr<nav2_msgs::srv::ClearEntireCostmap::Request> request,
const std::shared_ptr<nav2_msgs::srv::ClearEntireCostmap::Response> response);
- void clearLayerExceptRegion(
- std::shared_ptr<CostmapLayer> & costmap, double pose_x, double pose_y, double reset_distance);
-
- bool isClearable(const std::string & layer_name) const;
+ void clearLayerRegion(
+ std::shared_ptr<CostmapLayer> & costmap, double pose_x, double pose_y, double reset_distance,
+ bool invert);
bool getPosition(double & x, double & y) const;
-
- std::string getLayerName(const Layer & layer) const;
};
} // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
index af7a9948..5bdad73f 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
@@ -84,6 +84,7 @@ public:
virtual void activate() final;
virtual void deactivate() final;
virtual void reset() final;
+ virtual bool isClearable() {return false;}
/** CostmapFilter API **/
/**
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
index 86b293cb..5df24b54 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
@@ -60,7 +60,7 @@ public:
virtual void matchSize();
- virtual void clearArea(int start_x, int start_y, int end_x, int end_y);
+ virtual void clearArea(int start_x, int start_y, int end_x, int end_y, bool invert);
/**
* If an external source changes values in the costmap,
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
index 4acf05bd..225c0fbb 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
@@ -91,10 +91,12 @@ public:
int min_i, int min_j, int max_i, int max_j) override;
void matchSize() override;
+ virtual bool isClearable() {return false;}
void reset() override
{
matchSize();
+ current_ = false;
}
/** @brief Given a distance, compute a cost.
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
index 2012dbf8..467bfae5 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
@@ -68,6 +68,7 @@ public:
virtual void deactivate() {} /** @brief Stop publishers. */
virtual void activate() {} /** @brief Restart publishers if they've been stopped. */
virtual void reset() = 0;
+ virtual bool isClearable() = 0;
/**
* @brief This is called by the LayeredCostmap to poll this plugin as to how
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
index 8e56cb38..3e7413ac 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
@@ -83,6 +83,8 @@ public:
virtual void activate();
virtual void deactivate();
virtual void reset();
+ virtual bool isClearable() {return true;}
+
/**
* @brief triggers the update of observations buffer
*/
@@ -185,6 +187,7 @@ protected:
std::vector<nav2_costmap_2d::Observation> static_marking_observations_;
bool rolling_window_;
+ bool was_reset_;
int combination_method_;
};
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp
index ce286159..840ee20a 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp
@@ -75,6 +75,7 @@ public:
virtual void reset();
virtual void deactivate();
virtual void activate();
+ virtual bool isClearable() {return true;}
void bufferIncomingRangeMsg(const sensor_msgs::msg::Range::SharedPtr range_message);
@@ -116,6 +117,7 @@ private:
double clear_threshold_, mark_threshold_;
bool clear_on_max_reading_;
+ bool was_reset_;
tf2::Duration transform_tolerance_;
double no_readings_timeout_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
index bbd5daa7..e2be08d0 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
@@ -61,6 +61,7 @@ public:
virtual void activate();
virtual void deactivate();
virtual void reset();
+ virtual bool isClearable() {return false;}
virtual void updateBounds(
double robot_x, double robot_y, double robot_yaw, double * min_x,
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
index 92445cd7..22cf4269 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
@@ -80,6 +80,7 @@ public:
}
virtual void matchSize();
virtual void reset();
+ virtual bool isClearable() {return true;}
protected:
virtual void resetMaps();
diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
index 7d0d2971..dfed2d32 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
@@ -93,6 +93,7 @@ void CostmapFilter::reset()
{
resetFilter();
initializeFilter(filter_info_topic_);
+ current_ = false;
}
void CostmapFilter::updateBounds(
@@ -117,6 +118,7 @@ void CostmapFilter::updateCosts(
}
process(master_grid, min_i, min_j, max_i, max_j, latest_pose_);
+ current_ = true;
}
} // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp
index 0d7d82aa..de60b342 100644
--- a/nav2_costmap_2d/plugins/inflation_layer.cpp
+++ b/nav2_costmap_2d/plugins/inflation_layer.cpp
@@ -279,6 +279,8 @@ InflationLayer::updateCosts(
dist.clear();
dist.reserve(200);
}
+
+ current_ = true;
}
/**
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index d456be34..1a227961 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -108,6 +108,7 @@ void ObstacleLayer::onInitialize()
ObstacleLayer::matchSize();
current_ = true;
+ was_reset_ = false;
global_frame_ = layered_costmap_->getGlobalFrameID();
@@ -444,6 +445,12 @@ ObstacleLayer::updateCosts(
return;
}
+ // if not current due to reset, set current now after clearing
+ if (!current_ && was_reset_) {
+ was_reset_ = false;
+ current_ = true;
+ }
+
if (footprint_clearing_enabled_) {
setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE);
}
@@ -647,7 +654,8 @@ ObstacleLayer::reset()
{
resetMaps();
resetBuffersLastUpdated();
- current_ = true;
+ current_ = false;
+ was_reset_ = true;
}
void
diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp
index 3d0f728b..c8020781 100644
--- a/nav2_costmap_2d/plugins/range_sensor_layer.cpp
+++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp
@@ -60,6 +60,7 @@ RangeSensorLayer::RangeSensorLayer() {}
void RangeSensorLayer::onInitialize()
{
current_ = true;
+ was_reset_ = false;
buffered_readings_ = 0;
last_reading_time_ = clock_->now();
default_value_ = to_cost(0.5);
@@ -509,7 +510,12 @@ void RangeSensorLayer::updateCosts(
}
buffered_readings_ = 0;
- current_ = true;
+
+ // if not current due to reset, set current now after clearing
+ if (!current_ && was_reset_) {
+ was_reset_ = false;
+ current_ = true;
+ }
}
void RangeSensorLayer::reset()
@@ -517,7 +523,7 @@ void RangeSensorLayer::reset()
RCLCPP_DEBUG(logger_, "Reseting range sensor layer...");
deactivate();
resetMaps();
- current_ = true;
+ was_reset_ = true;
activate();
}
diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
index e503126a..37a5cef2 100644
--- a/nav2_costmap_2d/plugins/static_layer.cpp
+++ b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -116,6 +116,7 @@ void
StaticLayer::reset()
{
has_updated_data_ = true;
+ current_ = false;
}
void
@@ -431,6 +432,7 @@ StaticLayer::updateCosts(
}
}
update_in_progress_.store(false);
+ current_ = true;
}
} // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp
index 3ef9f633..61f89c1f 100644
--- a/nav2_costmap_2d/src/clear_costmap_service.cpp
+++ b/nav2_costmap_2d/src/clear_costmap_service.cpp
@@ -70,7 +70,7 @@ void ClearCostmapService::clearExceptRegionCallback(
logger_,
"Received request to clear except a region the " + costmap_.getName());
- clearExceptRegion(request->reset_distance);
+ clearRegion(request->reset_distance, true);
}
void ClearCostmapService::clearAroundRobotCallback(
@@ -78,16 +78,7 @@ void ClearCostmapService::clearAroundRobotCallback(
const shared_ptr<ClearAroundRobot::Request> request,
const shared_ptr<ClearAroundRobot::Response>/*response*/)
{
- RCLCPP_INFO(
- logger_,
- "Received request to clear around robot the " + costmap_.getName());
-
- if ((request->window_size_x == 0) || (request->window_size_y == 0)) {
- clearEntirely();
- return;
- }
-
- clearAroundRobot(request->window_size_x, request->window_size_y);
+ clearRegion(request->reset_distance, false);
}
void ClearCostmapService::clearEntireCallback(
@@ -102,7 +93,7 @@ void ClearCostmapService::clearEntireCallback(
clearEntirely();
}
-void ClearCostmapService::clearExceptRegion(const double reset_distance)
+void ClearCostmapService::clearRegion(const double reset_distance, bool invert)
{
double x, y;
@@ -116,59 +107,16 @@ void ClearCostmapService::clearExceptRegion(const double reset_distance)
auto layers = costmap_.getLayeredCostmap()->getPlugins();
for (auto & layer : *layers) {
- if (isClearable(getLayerName(*layer))) {
+ if (layer->isClearable()) {
auto costmap_layer = std::static_pointer_cast<CostmapLayer>(layer);
- clearLayerExceptRegion(costmap_layer, x, y, reset_distance);
+ clearLayerRegion(costmap_layer, x, y, reset_distance, invert);
}
}
}
-void ClearCostmapService::clearAroundRobot(double window_size_x, double window_size_y)
-{
- double pose_x, pose_y;
-
- if (!getPosition(pose_x, pose_y)) {
- RCLCPP_ERROR(
- logger_,
- "Cannot clear map because robot pose cannot be retrieved.");
- return;
- }
-
- std::vector<geometry_msgs::msg::Point> clear_poly;
- geometry_msgs::msg::Point pt;
-
- pt.x = pose_x - window_size_x / 2;
- pt.y = pose_y - window_size_y / 2;
- clear_poly.push_back(pt);
-
- pt.x = pose_x + window_size_x / 2;
- pt.y = pose_y - window_size_y / 2;
- clear_poly.push_back(pt);
-
- pt.x = pose_x + window_size_x / 2;
- pt.y = pose_y + window_size_y / 2;
- clear_poly.push_back(pt);
-
- pt.x = pose_x - window_size_x / 2;
- pt.y = pose_y + window_size_y / 2;
- clear_poly.push_back(pt);
-
- costmap_.getCostmap()->setConvexPolygonCost(clear_poly, reset_value_);
-}
-
-void ClearCostmapService::clearEntirely()
-{
- std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getCostmap()->getMutex()));
- costmap_.resetLayers();
-}
-
-bool ClearCostmapService::isClearable(const string & layer_name) const
-{
- return count(begin(clearable_layers_), end(clearable_layers_), layer_name) != 0;
-}
-
-void ClearCostmapService::clearLayerExceptRegion(
- shared_ptr<CostmapLayer> & costmap, double pose_x, double pose_y, double reset_distance)
+void ClearCostmapService::clearLayerRegion(
+ shared_ptr<CostmapLayer> & costmap, double pose_x, double pose_y, double reset_distance,
+ bool invert)
{
std::unique_lock<Costmap2D::mutex_t> lock(*(costmap->getMutex()));
@@ -181,24 +129,19 @@ void ClearCostmapService::clearLayerExceptRegion(
costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
- unsigned int size_x = costmap->getSizeInCellsX();
- unsigned int size_y = costmap->getSizeInCellsY();
-
- // Clearing the four rectangular regions around the one we want to keep
- // top region
- costmap->resetMapToValue(0, 0, size_x, start_y, reset_value_);
- // left region
- costmap->resetMapToValue(0, start_y, start_x, end_y, reset_value_);
- // right region
- costmap->resetMapToValue(end_x, start_y, size_x, end_y, reset_value_);
- // bottom region
- costmap->resetMapToValue(0, end_y, size_x, size_y, reset_value_);
+ costmap->clearArea(start_x, start_y, end_x, end_y, invert);
double ox = costmap->getOriginX(), oy = costmap->getOriginY();
double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();
costmap->addExtraBounds(ox, oy, ox + width, oy + height);
}
+void ClearCostmapService::clearEntirely()
+{
+ std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getCostmap()->getMutex()));
+ costmap_.resetLayers();
+}
+
bool ClearCostmapService::getPosition(double & x, double & y) const
{
geometry_msgs::msg::PoseStamped pose;
@@ -212,17 +155,4 @@ bool ClearCostmapService::getPosition(double & x, double & y) const
return true;
}
-string ClearCostmapService::getLayerName(const Layer & layer) const
-{
- string name = layer.getName();
-
- size_t slash = name.rfind('/');
-
- if (slash != std::string::npos) {
- name = name.substr(slash + 1);
- }
-
- return name;
-}
-
} // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp
index b3aecb75..1179454d 100644
--- a/nav2_costmap_2d/src/costmap_layer.cpp
+++ b/nav2_costmap_2d/src/costmap_layer.cpp
@@ -61,14 +61,15 @@ void CostmapLayer::matchSize()
master->getOriginX(), master->getOriginY());
}
-void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y)
+void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert)
{
+ current_ = false;
unsigned char * grid = getCharMap();
for (int x = 0; x < static_cast<int>(getSizeInCellsX()); x++) {
bool xrange = x > start_x && x < end_x;
for (int y = 0; y < static_cast<int>(getSizeInCellsY()); y++) {
- if (xrange && y > start_y && y < end_y) {
+ if ((xrange && y > start_y && y < end_y) != invert) {
continue;
}
int index = getIndex(x, y);
diff --git a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp
index dea1bb35..ae4649e4 100644
--- a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp
+++ b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp
@@ -33,6 +33,7 @@ class LayerWrapper : public nav2_costmap_2d::Layer
void reset() {}
void updateBounds(double, double, double, double *, double *, double *, double *) {}
void updateCosts(nav2_costmap_2d::Costmap2D &, int, int, int, int) {}
+ bool isClearable() {return false;}
};
TEST(DeclareParameter, useValidParameter2Args)
diff --git a/nav2_msgs/srv/ClearCostmapAroundRobot.srv b/nav2_msgs/srv/ClearCostmapAroundRobot.srv
index 150ca613..e3b41bf5 100644
--- a/nav2_msgs/srv/ClearCostmapAroundRobot.srv
+++ b/nav2_msgs/srv/ClearCostmapAroundRobot.srv
@@ -1,6 +1,5 @@
-# Clears the costmap within a window around the robot
+# Clears the costmap within a distance
-float32 window_size_x
-float32 window_size_y
+float32 reset_distance
---
std_msgs/Empty response
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
index 47c4e4aa..7aa925c7 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -225,6 +225,12 @@ PlannerServer::computePlan()
return;
}
+ // Don't compute a plan until costmap is valid (after clear costmap)
+ rclcpp::Rate r(100);
+ while (!costmap_ros_->isCurrent()) {
+ r.sleep();
+ }
+
geometry_msgs::msg::PoseStamped start;
if (!costmap_ros_->getRobotPose(start)) {
action_server_->terminate_current();
--
GitLab