38 #include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
43 #include "tf2/convert.hpp"
44 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
46 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
47 #include "nav2_util/geometry_utils.hpp"
48 #include "nav2_util/occ_grid_utils.hpp"
49 #include "nav2_util/robot_utils.hpp"
55 : filter_info_sub_(nullptr), mask_sub_(nullptr), filter_mask_(nullptr),
61 const std::string & filter_info_topic)
63 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
65 nav2::LifecycleNode::SharedPtr node = node_.lock();
67 throw std::runtime_error{
"Failed to lock node"};
74 "KeepoutFilter: Subscribing to \"%s\" topic for filter info...",
76 filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
81 global_frame_ = layered_costmap_->getGlobalFrameID();
83 override_lethal_cost_ = node->declare_or_get_parameter(name_ +
"." +
"override_lethal_cost",
85 lethal_override_cost_ = node->declare_or_get_parameter(name_ +
"." +
"lethal_override_cost",
89 lethal_override_cost_ = \
90 std::clamp<unsigned int>(lethal_override_cost_, FREE_SPACE, MAX_NON_OBSTACLE);
91 lethal_state_update_max_x_ = lethal_state_update_max_y_ = std::numeric_limits<double>::lowest();
92 lethal_state_update_min_x_ = lethal_state_update_min_y_ = std::numeric_limits<double>::max();
96 const nav2_msgs::msg::CostmapFilterInfo::ConstSharedPtr & msg)
98 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
100 nav2::LifecycleNode::SharedPtr node = node_.lock();
102 throw std::runtime_error{
"Failed to lock node"};
112 "KeepoutFilter: New costmap filter info arrived from %s topic. Updating old filter info.",
119 if (msg->base != BASE_DEFAULT || msg->multiplier != MULTIPLIER_DEFAULT) {
122 "KeepoutFilter: For proper use of keepout filter base and multiplier"
123 " in CostmapFilterInfo message should be set to their default values (%f and %f)",
124 BASE_DEFAULT, MULTIPLIER_DEFAULT);
132 "KeepoutFilter: Subscribing to \"%s\" topic for filter mask...",
134 mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
141 const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & msg)
143 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
145 nav2::LifecycleNode::SharedPtr node = node_.lock();
147 throw std::runtime_error{
"Failed to lock node"};
153 "KeepoutFilter: Received filter mask from %s topic.",
mask_topic_.c_str());
157 "KeepoutFilter: New filter mask arrived from %s topic. Updating old filter mask.",
159 filter_mask_.reset();
164 has_updated_data_ =
true;
166 width_ = msg->info.width;
167 height_ = msg->info.height;
171 double robot_x,
double robot_y,
double robot_yaw,
172 double * min_x,
double * min_y,
double * max_x,
double * max_y)
181 RCLCPP_WARN_THROTTLE(
182 logger_, *(clock_), 2000,
183 "KeepoutFilter: Filter mask was not received");
188 if (has_updated_data_) {
191 *min_x = std::min(wx, *min_x);
192 *min_y = std::min(wy, *min_y);
195 *max_x = std::max(wx, *max_x);
196 *max_y = std::max(wy, *max_y);
198 has_updated_data_ =
false;
203 is_pose_lethal_ =
false;
204 if (override_lethal_cost_) {
205 geometry_msgs::msg::Pose pose;
206 pose.position.x = robot_x;
207 pose.position.y = robot_y;
208 pose.position.z = 0.0;
209 pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(robot_yaw);
210 geometry_msgs::msg::Pose mask_pose;
211 geometry_msgs::msg::TransformStamped mask_transform;
212 if (nav2_util::lookupTransformWithStalenessCheck(
213 *tf_, filter_mask_->header.frame_id, global_frame_, clock_->now(),
216 tf2::doTransform(pose, mask_pose, mask_transform);
217 unsigned int mask_robot_i, mask_robot_j;
218 if (nav2_util::worldToMap(
219 filter_mask_, mask_pose.position.x, mask_pose.position.y,
220 mask_robot_i, mask_robot_j))
222 auto data =
getMaskCost(filter_mask_, mask_robot_i, mask_robot_j);
223 is_pose_lethal_ = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE);
224 if (is_pose_lethal_) {
225 RCLCPP_WARN_THROTTLE(
226 logger_, *(clock_), 2000,
227 "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out.");
234 if (is_pose_lethal_ || (last_pose_lethal_ && !is_pose_lethal_)) {
235 lethal_state_update_min_x_ = std::min(*min_x, lethal_state_update_min_x_);
236 *min_x = lethal_state_update_min_x_;
237 lethal_state_update_min_y_ = std::min(*min_y, lethal_state_update_min_y_);
238 *min_y = lethal_state_update_min_y_;
239 lethal_state_update_max_x_ = std::max(*max_x, lethal_state_update_max_x_);
240 *max_x = lethal_state_update_max_x_;
241 lethal_state_update_max_y_ = std::max(*max_y, lethal_state_update_max_y_);
242 *max_y = lethal_state_update_max_y_;
245 lethal_state_update_min_x_ = std::numeric_limits<double>::max();
246 lethal_state_update_min_y_ = std::numeric_limits<double>::max();
247 lethal_state_update_max_x_ = std::numeric_limits<double>::lowest();
248 lethal_state_update_max_y_ = std::numeric_limits<double>::lowest();
255 int min_i,
int min_j,
int max_i,
int max_j,
256 const geometry_msgs::msg::Pose & )
258 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
262 RCLCPP_WARN_THROTTLE(
263 logger_, *(clock_), 2000,
264 "KeepoutFilter: Filter mask was not received");
268 tf2::Transform tf2_transform;
269 tf2_transform.setIdentity();
270 int mg_min_x, mg_min_y;
271 int mg_max_x, mg_max_y;
273 const std::string mask_frame = filter_mask_->header.frame_id;
275 if (mask_frame != global_frame_) {
278 geometry_msgs::msg::TransformStamped mask_transform;
279 if (!nav2_util::lookupTransformWithStalenessCheck(
280 *tf_, mask_frame, global_frame_, clock_->now(),
285 tf2::fromMsg(mask_transform.transform, tf2_transform);
316 const double half_cell_size = 0.5 * filter_mask_->info.resolution;
317 wx = filter_mask_->info.origin.position.x + half_cell_size;
318 wy = filter_mask_->info.origin.position.y + half_cell_size;
321 if (mg_min_x >= max_i || mg_min_y >= max_j) {
325 mg_min_x = std::max(min_i, mg_min_x);
326 mg_min_y = std::max(min_j, mg_min_y);
330 wx = filter_mask_->info.origin.position.x +
331 filter_mask_->info.width * filter_mask_->info.resolution + half_cell_size;
332 wy = filter_mask_->info.origin.position.y +
333 filter_mask_->info.height * filter_mask_->info.resolution + half_cell_size;
336 if (mg_max_x <= min_i || mg_max_y <= min_j) {
340 mg_max_x = std::min(max_i, mg_max_x);
341 mg_max_y = std::min(max_j, mg_max_y);
345 const unsigned int mg_min_x_u =
static_cast<unsigned int>(mg_min_x);
346 const unsigned int mg_min_y_u =
static_cast<unsigned int>(mg_min_y);
347 const unsigned int mg_max_x_u =
static_cast<unsigned int>(mg_max_x);
348 const unsigned int mg_max_y_u =
static_cast<unsigned int>(mg_max_y);
353 double msk_wx, msk_wy;
355 unsigned char data, old_data;
359 unsigned char * master_array = master_grid.
getCharMap();
360 for (i = mg_min_x_u; i < mg_max_x_u; i++) {
361 for (j = mg_min_y_u; j < mg_max_y_u; j++) {
363 old_data = master_array[index];
367 if (mask_frame != global_frame_) {
369 tf2::Vector3 point(gl_wx, gl_wy, 0);
370 point = tf2_transform * point;
379 if (nav2_util::worldToMap(filter_mask_, msk_wx, msk_wy, mx, my)) {
382 if (data == NO_INFORMATION) {
386 if (data > old_data || old_data == NO_INFORMATION) {
387 if (old_data == NO_INFORMATION && master_grid.
getDefaultValue() == NO_INFORMATION) {
390 "KeepoutFilter: keepout mask is overwriting unknown space with cost %u. "
391 "Set the mask's background to unknown if that's not what you want.",
394 if (override_lethal_cost_ && is_pose_lethal_) {
395 master_array[index] = lethal_override_cost_;
397 master_array[index] = data;
404 last_pose_lethal_ = is_pose_lethal_;
409 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
411 filter_info_sub_.reset();
417 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
427 #include "pluginlib/class_list_macros.hpp"
A QoS profile for latched, reliable topics with a history of 10 messages.
A 2D costmap provides a mapping between points in the world and their associated "costs".
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
Convert from map coordinates to world coordinates.
unsigned int getIndex(unsigned int mx, unsigned int my) const
Given two map coordinates... compute the associated index.
unsigned char * getCharMap() const
Will return a pointer to the underlying unsigned char array used as the costmap.
void worldToMapNoBounds(double wx, double wy, int &mx, int &my) const
Convert from world coordinates to map coordinates without checking for legal bounds.
unsigned char getDefaultValue()
Get the default background value of the costmap.
double transform_staleness_threshold_
: Max age for transforms. Check disabled if <= 0.
std::string filter_info_topic_
: Name of costmap filter info topic
std::string mask_topic_
: Name of filter mask topic
unsigned char getMaskCost(nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int &my) const
Get the cost of a cell in the filter mask.
mutex_t * getMutex()
: returns pointer to a mutex
void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y) override
Update the bounds of the master costmap by this layer's update dimensions.
Reads in a keepout mask and marks keepout regions in the map to prevent planning or control in restri...
void initializeFilter(const std::string &filter_info_topic) override
Initialize the filter and subscribe to the info topic.
void resetFilter() override
Reset the costmap filter / topic / info.
void process(nav2_costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j, const geometry_msgs::msg::Pose &pose) override
Process the keepout layer at the current pose / bounds / grid.
void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::ConstSharedPtr &msg)
Callback for the filter information.
KeepoutFilter()
A constructor.
bool isActive()
If this filter is active.
void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y) override
Update the bounds of the master costmap by this layer's update dimensions.
void maskCallback(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr &msg)
Callback for the filter mask.
Abstract class for layered costmap plugin implementations.
std::string joinWithParentNamespace(const std::string &topic)
Costmap2D * getCostmap()
Get the costmap pointer to the master costmap.