From 43d9b0afa152698ee77d03f37be80d2f7f11e73e Mon Sep 17 00:00:00 2001
From: "Carlos A. Orduno" <39749557+orduno@users.noreply.github.com>
Date: Mon, 6 May 2019 15:13:48 -0700
Subject: [PATCH] Adding a service to clear a costmap around the robot (#596)
* Adding a service to clear a costmap around the robot
* Addressed code review feedback.
---
.../nav2_costmap_2d/clear_costmap_service.hpp | 10 ++++
nav2_costmap_2d/src/clear_costmap_service.cpp | 53 +++++++++++++++++++
nav2_msgs/CMakeLists.txt | 1 +
nav2_msgs/srv/ClearCostmapAroundRobot.srv | 6 +++
4 files changed, 70 insertions(+)
create mode 100644 nav2_msgs/srv/ClearCostmapAroundRobot.srv
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 1334cec8..d9829dba 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
@@ -21,6 +21,7 @@
#include "rclcpp/rclcpp.hpp"
#include "nav2_msgs/srv/clear_costmap_except_region.hpp"
+#include "nav2_msgs/srv/clear_costmap_around_robot.hpp"
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
#include "nav2_costmap_2d/costmap_layer.hpp"
@@ -39,6 +40,9 @@ public:
// 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);
+
// Clears all layers
void clearEntirely();
@@ -61,6 +65,12 @@ private:
const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Request> request,
const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Response> response);
+ rclcpp::Service<nav2_msgs::srv::ClearCostmapAroundRobot>::SharedPtr clear_around_service_;
+ void clearAroundRobotCallback(
+ const std::shared_ptr<rmw_request_id_t> request_header,
+ const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Request> request,
+ const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Response> response);
+
rclcpp::Service<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr clear_entire_service_;
void clearEntireCallback(
const std::shared_ptr<rmw_request_id_t> request_header,
diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp
index db5acb3c..81c9281f 100644
--- a/nav2_costmap_2d/src/clear_costmap_service.cpp
+++ b/nav2_costmap_2d/src/clear_costmap_service.cpp
@@ -28,6 +28,7 @@ using std::string;
using std::shared_ptr;
using std::any_of;
using ClearExceptRegion = nav2_msgs::srv::ClearCostmapExceptRegion;
+using ClearAroundRobot = nav2_msgs::srv::ClearCostmapAroundRobot;
using ClearEntirely = nav2_msgs::srv::ClearEntireCostmap;
ClearCostmapService::ClearCostmapService(rclcpp::Node::SharedPtr & node, Costmap2DROS & costmap)
@@ -42,6 +43,11 @@ ClearCostmapService::ClearCostmapService(rclcpp::Node::SharedPtr & node, Costmap
std::bind(&ClearCostmapService::clearExceptRegionCallback, this,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+ clear_around_service_ = node_->create_service<ClearAroundRobot>(
+ "clear_around_" + costmap.getName(),
+ std::bind(&ClearCostmapService::clearAroundRobotCallback, this,
+ std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
clear_entire_service_ = node_->create_service<ClearEntirely>(
"clear_entirely_" + costmap_.getName(),
std::bind(&ClearCostmapService::clearEntireCallback, this,
@@ -59,6 +65,22 @@ void ClearCostmapService::clearExceptRegionCallback(
clearExceptRegion(request->reset_distance);
}
+void ClearCostmapService::clearAroundRobotCallback(
+ const shared_ptr<rmw_request_id_t>/*request_header*/,
+ const shared_ptr<ClearAroundRobot::Request> request,
+ const shared_ptr<ClearAroundRobot::Response>/*response*/)
+{
+ RCLCPP_INFO(node_->get_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);
+}
+
void ClearCostmapService::clearEntireCallback(
const std::shared_ptr<rmw_request_id_t>/*request_header*/,
const std::shared_ptr<ClearEntirely::Request>/*request*/,
@@ -88,6 +110,37 @@ void ClearCostmapService::clearExceptRegion(const double reset_distance)
}
}
+void ClearCostmapService::clearAroundRobot(double window_size_x, double window_size_y)
+{
+ double pose_x, pose_y;
+
+ if (!getPosition(pose_x, pose_y)) {
+ RCLCPP_ERROR(node_->get_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()));
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index d2fe0fcc..7231188f 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/VoxelGrid.msg"
"srv/GetCostmap.srv"
"srv/ClearCostmapExceptRegion.srv"
+ "srv/ClearCostmapAroundRobot.srv"
"srv/ClearEntireCostmap.srv"
DEPENDENCIES builtin_interfaces geometry_msgs std_msgs
)
diff --git a/nav2_msgs/srv/ClearCostmapAroundRobot.srv b/nav2_msgs/srv/ClearCostmapAroundRobot.srv
new file mode 100644
index 00000000..150ca613
--- /dev/null
+++ b/nav2_msgs/srv/ClearCostmapAroundRobot.srv
@@ -0,0 +1,6 @@
+# Clears the costmap within a window around the robot
+
+float32 window_size_x
+float32 window_size_y
+---
+std_msgs/Empty response
--
GitLab