Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
keepout_filter.cpp
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2020 Samsung Research Russia
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the <ORGANIZATION> nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: Alexey Merzlyakov
36  *********************************************************************/
37 
38 #include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
39 
40 #include <string>
41 #include <memory>
42 #include <algorithm>
43 #include "tf2/convert.hpp"
44 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
45 
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"
50 
51 namespace nav2_costmap_2d
52 {
53 
55 : filter_info_sub_(nullptr), mask_sub_(nullptr), filter_mask_(nullptr),
56  global_frame_("")
57 {
58 }
59 
61  const std::string & filter_info_topic)
62 {
63  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
64 
65  nav2::LifecycleNode::SharedPtr node = node_.lock();
66  if (!node) {
67  throw std::runtime_error{"Failed to lock node"};
68  }
69 
70  filter_info_topic_ = joinWithParentNamespace(filter_info_topic);
71  // Setting new costmap filter info subscriber
72  RCLCPP_INFO(
73  logger_,
74  "KeepoutFilter: Subscribing to \"%s\" topic for filter info...",
75  filter_info_topic_.c_str());
76  filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
78  std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1),
80 
81  global_frame_ = layered_costmap_->getGlobalFrameID();
82 
83  override_lethal_cost_ = node->declare_or_get_parameter(name_ + "." + "override_lethal_cost",
84  false);
85  lethal_override_cost_ = node->declare_or_get_parameter(name_ + "." + "lethal_override_cost",
86  MAX_NON_OBSTACLE);
87 
88  // clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given
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();
93 }
94 
96  const nav2_msgs::msg::CostmapFilterInfo::ConstSharedPtr & msg)
97 {
98  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
99 
100  nav2::LifecycleNode::SharedPtr node = node_.lock();
101  if (!node) {
102  throw std::runtime_error{"Failed to lock node"};
103  }
104 
105  if (!mask_sub_) {
106  RCLCPP_INFO(
107  logger_,
108  "KeepoutFilter: Received filter info from %s topic.", filter_info_topic_.c_str());
109  } else {
110  RCLCPP_WARN(
111  logger_,
112  "KeepoutFilter: New costmap filter info arrived from %s topic. Updating old filter info.",
113  filter_info_topic_.c_str());
114  // Resetting previous subscriber each time when new costmap filter information arrives
115  mask_sub_.reset();
116  }
117 
118  // Checking that base and multiplier are set to their default values
119  if (msg->base != BASE_DEFAULT || msg->multiplier != MULTIPLIER_DEFAULT) {
120  RCLCPP_ERROR(
121  logger_,
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);
125  }
126 
127  mask_topic_ = joinWithParentNamespace(msg->filter_mask_topic);
128 
129  // Setting new filter mask subscriber
130  RCLCPP_INFO(
131  logger_,
132  "KeepoutFilter: Subscribing to \"%s\" topic for filter mask...",
133  mask_topic_.c_str());
134  mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
135  mask_topic_,
136  std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1),
138 }
139 
141  const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & msg)
142 {
143  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
144 
145  nav2::LifecycleNode::SharedPtr node = node_.lock();
146  if (!node) {
147  throw std::runtime_error{"Failed to lock node"};
148  }
149 
150  if (!filter_mask_) {
151  RCLCPP_INFO(
152  logger_,
153  "KeepoutFilter: Received filter mask from %s topic.", mask_topic_.c_str());
154  } else {
155  RCLCPP_WARN(
156  logger_,
157  "KeepoutFilter: New filter mask arrived from %s topic. Updating old filter mask.",
158  mask_topic_.c_str());
159  filter_mask_.reset();
160  }
161 
162  // Store filter_mask_
163  filter_mask_ = msg;
164  has_updated_data_ = true;
165  x_ = y_ = 0;
166  width_ = msg->info.width;
167  height_ = msg->info.height;
168 }
169 
171  double robot_x, double robot_y, double robot_yaw,
172  double * min_x, double * min_y, double * max_x, double * max_y)
173 {
174  if (!enabled_) {
175  return;
176  }
177 
178  CostmapFilter::updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
179 
180  if (!filter_mask_) {
181  RCLCPP_WARN_THROTTLE(
182  logger_, *(clock_), 2000,
183  "KeepoutFilter: Filter mask was not received");
184  return;
185  }
186 
187  // If new keepout zone received
188  if (has_updated_data_) {
189  double wx, wy;
190  layered_costmap_->getCostmap()->mapToWorld(x_, y_, wx, wy);
191  *min_x = std::min(wx, *min_x);
192  *min_y = std::min(wy, *min_y);
193 
194  layered_costmap_->getCostmap()->mapToWorld(x_ + width_, y_ + height_, wx, wy);
195  *max_x = std::max(wx, *max_x);
196  *max_y = std::max(wy, *max_y);
197 
198  has_updated_data_ = false;
199  return;
200  }
201 
202  // Let's find the pose's cost if we are allowed to override the lethal cost
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(),
214  transform_staleness_threshold_, mask_transform))
215  {
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))
221  {
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.");
228  }
229  }
230  }
231 
232  // If in lethal space or just exited lethal space,
233  // we need to update all possible spaces touched during this state
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_;
243  } else {
244  // If out of lethal space, reset managed lethal state sizes
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();
249  }
250  }
251 }
252 
254  nav2_costmap_2d::Costmap2D & master_grid,
255  int min_i, int min_j, int max_i, int max_j,
256  const geometry_msgs::msg::Pose & /*pose*/)
257 {
258  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
259 
260  if (!filter_mask_) {
261  // Show warning message every 2 seconds to not litter an output
262  RCLCPP_WARN_THROTTLE(
263  logger_, *(clock_), 2000,
264  "KeepoutFilter: Filter mask was not received");
265  return;
266  }
267 
268  tf2::Transform tf2_transform;
269  tf2_transform.setIdentity(); // initialize by identical transform
270  int mg_min_x, mg_min_y; // master_grid indexes of bottom-left window corner
271  int mg_max_x, mg_max_y; // master_grid indexes of top-right window corner
272 
273  const std::string mask_frame = filter_mask_->header.frame_id;
274 
275  if (mask_frame != global_frame_) {
276  // Filter mask and current layer are in different frames:
277  // prepare frame transformation 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(),
281  transform_staleness_threshold_, mask_transform))
282  {
283  return;
284  }
285  tf2::fromMsg(mask_transform.transform, tf2_transform);
286 
287  mg_min_x = min_i;
288  mg_min_y = min_j;
289  mg_max_x = max_i;
290  mg_max_y = max_j;
291  } else {
292  // Filter mask and current layer are in the same frame:
293  // apply the following optimization - iterate only in overlapped
294  // (min_i, min_j)..(max_i, max_j) & filter_mask_ area.
295  //
296  // filter_mask_
297  // *----------------------------*
298  // | |
299  // | |
300  // | (2) |
301  // *-----+-------* |
302  // | |///////|<- overlapped area |
303  // | |///////| to iterate in |
304  // | *-------+--------------------*
305  // | (1) |
306  // | |
307  // *-------------*
308  // master_grid (min_i, min_j)..(max_i, max_j) window
309  //
310  // ToDo: after costmap rotation will be added, this should be re-worked.
311 
312  double wx, wy; // world coordinates
313 
314  // Calculating bounds corresponding to bottom-left overlapping (1) corner
315  // filter_mask_ -> master_grid indexes conversion
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;
319  master_grid.worldToMapNoBounds(wx, wy, mg_min_x, mg_min_y);
320  // Calculation of (1) corner bounds
321  if (mg_min_x >= max_i || mg_min_y >= max_j) {
322  // There is no overlapping. Do nothing.
323  return;
324  }
325  mg_min_x = std::max(min_i, mg_min_x);
326  mg_min_y = std::max(min_j, mg_min_y);
327 
328  // Calculating bounds corresponding to top-right window (2) corner
329  // filter_mask_ -> master_grid indexes conversion
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;
334  master_grid.worldToMapNoBounds(wx, wy, mg_max_x, mg_max_y);
335  // Calculation of (2) corner bounds
336  if (mg_max_x <= min_i || mg_max_y <= min_j) {
337  // There is no overlapping. Do nothing.
338  return;
339  }
340  mg_max_x = std::min(max_i, mg_max_x);
341  mg_max_y = std::min(max_j, mg_max_y);
342  }
343 
344  // unsigned<-signed conversions.
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);
349 
350  unsigned int i, j; // master_grid iterators
351  unsigned int index; // corresponding index of master_grid
352  double gl_wx, gl_wy; // world coordinates in a global_frame_
353  double msk_wx, msk_wy; // world coordinates in a mask_frame
354  unsigned int mx, my; // filter_mask_ coordinates
355  unsigned char data, old_data; // master_grid element data
356 
357  // Main master_grid updating loop
358  // Iterate in costmap window by master_grid indexes
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++) {
362  index = master_grid.getIndex(i, j);
363  old_data = master_array[index];
364  // Calculating corresponding to (i, j) point at filter_mask_:
365  // Get world coordinates in global_frame_
366  master_grid.mapToWorld(i, j, gl_wx, gl_wy);
367  if (mask_frame != global_frame_) {
368  // Transform (i, j) point from global_frame_ to mask_frame
369  tf2::Vector3 point(gl_wx, gl_wy, 0);
370  point = tf2_transform * point;
371  msk_wx = point.x();
372  msk_wy = point.y();
373  } else {
374  // In this case master_grid and filter-mask are in the same frame
375  msk_wx = gl_wx;
376  msk_wy = gl_wy;
377  }
378  // Get mask coordinates corresponding to (i, j) point at filter_mask_
379  if (nav2_util::worldToMap(filter_mask_, msk_wx, msk_wy, mx, my)) {
380  data = getMaskCost(filter_mask_, mx, my);
381  // Update if mask_ data is valid and greater than existing master_grid's one
382  if (data == NO_INFORMATION) {
383  continue;
384  }
385 
386  if (data > old_data || old_data == NO_INFORMATION) {
387  if (old_data == NO_INFORMATION && master_grid.getDefaultValue() == NO_INFORMATION) {
388  RCLCPP_WARN_ONCE(
389  logger_,
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.",
392  data);
393  }
394  if (override_lethal_cost_ && is_pose_lethal_) {
395  master_array[index] = lethal_override_cost_;
396  } else {
397  master_array[index] = data;
398  }
399  }
400  }
401  }
402  }
403 
404  last_pose_lethal_ = is_pose_lethal_;
405 }
406 
408 {
409  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
410 
411  filter_info_sub_.reset();
412  mask_sub_.reset();
413 }
414 
416 {
417  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
418 
419  if (filter_mask_) {
420  return true;
421  }
422  return false;
423 }
424 
425 } // namespace nav2_costmap_2d
426 
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".
Definition: costmap_2d.hpp:69
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
Convert from map coordinates to world coordinates.
Definition: costmap_2d.cpp:280
unsigned int getIndex(unsigned int mx, unsigned int my) const
Given two map coordinates... compute the associated index.
Definition: costmap_2d.hpp:231
unsigned char * getCharMap() const
Will return a pointer to the underlying unsigned char array used as the costmap.
Definition: costmap_2d.cpp:260
void worldToMapNoBounds(double wx, double wy, int &mx, int &my) const
Convert from world coordinates to map coordinates without checking for legal bounds.
Definition: costmap_2d.cpp:322
unsigned char getDefaultValue()
Get the default background value of the costmap.
Definition: costmap_2d.hpp:309
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.
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.
Definition: layer.hpp:62
std::string joinWithParentNamespace(const std::string &topic)
Definition: layer.cpp:83
Costmap2D * getCostmap()
Get the costmap pointer to the master costmap.