forked from open-navigation/navigation2
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Costmap Filters (open-navigation#1882)
* Add costmap filters with KeepoutFilter realization * Cover preferred lanes in industries by KeepoutFilter (open-navigation#1522) * * Resolve review questions * Add new OccupancyGrid class into nav2_util - API wrapper for original OccupancyGrid * Add declareParameter() with one argument to Layer API * Resolve 2nd review questions * Remove OccupancyGrid class wrapper * Add Costmap2D constructor with OccupancyGrid instead * Add launch-file to help running CostmapFilterInfo publisher * Code clean-up * Resolve 3rd review questions (partially) * Fix hidden problems during the shared handling of copstmap_ resource * Cast into double during the linear conversion of OccupancyGrid data * Add keepout_filter system test * Add error message when base or multiplier is not set in ConstmapFilterInfo message * Add comment to dummy info publisher launch-file * Add costmap_conversion unit test * Add declareParameter() unit test * Add keepout zone entering check in FiltersTester * Fix upper window boundary calculation in KeepoutFilter * Correct mutex header in costmap_filters * Added KeepoutFilter::isActive() method for filter readiness check * Add KeepoutFilter unit test * Correct the code to consider node_ as a weak pointer * Enhance KeepoutFilter unit test coverage by adding different surfaces * Made CostmapFilters and minor documentation updates * Fixed typo * Move CostmapFilterInfo publisher to Map Server * Add costmap_filter_info.launch.py into tb3_simulation_launch.py * Remove hard-coded bar areas from test_keepout_filter * Add CostmapFilters HLD * Fix doc after review * Fix mask parameter initialization in bringup_launch.py * Improve tests and remove costmap-filters from nav2_bringup * Update map mask to filter mask in HLD * Small fix-ups * Fix error message
- Loading branch information
1 parent
71ef9ff
commit 6b40c2d
Showing
39 changed files
with
2,404 additions
and
22 deletions.
There are no files selected for viewing
Binary file not shown.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
141 changes: 141 additions & 0 deletions
141
nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,141 @@ | ||
| /********************************************************************* | ||
| * | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Copyright (c) 2020 Samsung Research Russia | ||
| * All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without | ||
| * modification, are permitted provided that the following conditions | ||
| * are met: | ||
| * | ||
| * * Redistributions of source code must retain the above copyright | ||
| * notice, this list of conditions and the following disclaimer. | ||
| * * Redistributions in binary form must reproduce the above | ||
| * copyright notice, this list of conditions and the following | ||
| * disclaimer in the documentation and/or other materials provided | ||
| * with the distribution. | ||
| * * Neither the name of the <ORGANIZATION> nor the names of its | ||
| * contributors may be used to endorse or promote products derived | ||
| * from this software without specific prior written permission. | ||
| * | ||
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| * POSSIBILITY OF SUCH DAMAGE. | ||
| * | ||
| * Author: Alexey Merzlyakov | ||
| *********************************************************************/ | ||
|
|
||
| #ifndef NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_ | ||
| #define NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_ | ||
|
|
||
| #include <string> | ||
| #include <mutex> | ||
|
|
||
| #include "geometry_msgs/msg/pose2_d.hpp" | ||
| #include "nav2_costmap_2d/layer.hpp" | ||
|
|
||
| namespace nav2_costmap_2d | ||
| { | ||
|
|
||
| static constexpr double BASE_DEFAULT = 0.0; | ||
| static constexpr double MULTIPLIER_DEFAULT = 1.0; | ||
|
|
||
| /** | ||
| * @brief: CostmapFilter basic class. It is inherited from Layer in order to avoid | ||
| * hidden problems when the shared handling of costmap_ resource (PR #1936) | ||
| */ | ||
| class CostmapFilter : public Layer | ||
| { | ||
| public: | ||
| CostmapFilter(); | ||
| ~CostmapFilter(); | ||
|
|
||
| /** | ||
| * @brief: Provide a typedef to ease future code maintenance | ||
| */ | ||
| typedef std::recursive_mutex mutex_t; | ||
| /** | ||
| * @brief: returns pointer to a mutex | ||
| */ | ||
| mutex_t * getMutex() | ||
| { | ||
| return access_; | ||
| } | ||
|
|
||
| /** Layer API **/ | ||
| virtual void onInitialize() final; | ||
| virtual void updateBounds( | ||
| double robot_x, double robot_y, double robot_yaw, | ||
| double * min_x, double * min_y, double * max_x, double * max_y) final; | ||
| virtual void updateCosts( | ||
| nav2_costmap_2d::Costmap2D & master_grid, | ||
| int min_i, int min_j, int max_i, int max_j) final; | ||
|
|
||
| virtual void activate() final; | ||
| virtual void deactivate() final; | ||
| virtual void reset() final; | ||
|
|
||
| /** CostmapFilter API **/ | ||
| /** | ||
| * @brief: Initializes costmap filter. Creates subscriptions to filter-related topics | ||
| * @param: Name of costmap filter info topic | ||
| */ | ||
| virtual void initializeFilter( | ||
| const std::string & filter_info_topic) = 0; | ||
|
|
||
| /** | ||
| * @brief: An algorithm for how to use that map's information. Fills the Costmap2D with | ||
| * calculated data and makes an action based on processed data | ||
| * @param: Reference to a master costmap2d | ||
| * @param: Low window map boundary OX | ||
| * @param: Low window map boundary OY | ||
| * @param: High window map boundary OX | ||
| * @param: High window map boundary OY | ||
| * @param: Robot 2D-pose | ||
| */ | ||
| virtual void process( | ||
| nav2_costmap_2d::Costmap2D & master_grid, | ||
| int min_i, int min_j, int max_i, int max_j, | ||
| const geometry_msgs::msg::Pose2D & pose) = 0; | ||
|
|
||
| /** | ||
| * @brief: Resets costmap filter. Stops all subscriptions | ||
| */ | ||
| virtual void resetFilter() = 0; | ||
|
|
||
| protected: | ||
| /** | ||
| * @brief: Name of costmap filter info topic | ||
| */ | ||
| std::string filter_info_topic_; | ||
|
|
||
| /** | ||
| * @brief: Name of filter mask topic | ||
| */ | ||
| std::string mask_topic_; | ||
|
|
||
| private: | ||
| /** | ||
| * @brief: Latest robot position | ||
| */ | ||
| geometry_msgs::msg::Pose2D latest_pose_; | ||
|
|
||
| /** | ||
| * @brief: Mutex for locking filter's resources | ||
| */ | ||
| mutex_t * access_; | ||
| }; | ||
|
|
||
| } // namespace nav2_costmap_2d | ||
|
|
||
| #endif // NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_ |
Oops, something went wrong.