diff --git a/doc/design/CostmapFilters_design.pdf b/doc/design/CostmapFilters_design.pdf new file mode 100644 index 00000000000..016395c7eef Binary files /dev/null and b/doc/design/CostmapFilters_design.pdf differ diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index b0dfd7957c7..704ad4ddd01 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -72,7 +72,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo ## static_layer plugin -* ``: Name corresponding to the `nav2_costmap_2d::StaticLayer` plugin. This name gets defined in `plugin_names`, default value is `static_layer` +* ``: Name corresponding to the `nav2_costmap_2d::StaticLayer` plugin. This name gets defined in `plugins`, default value is `static_layer` | Parameter | Default | Description | | ----------| --------| ------------| @@ -84,7 +84,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo ## inflation_layer plugin -* ``: Name corresponding to the `nav2_costmap_2d::InflationLayer` plugin. This name gets defined in `plugin_names`, default value is `inflation_layer` +* ``: Name corresponding to the `nav2_costmap_2d::InflationLayer` plugin. This name gets defined in `plugins`, default value is `inflation_layer` | Parameter | Default | Description | | ----------| --------| ------------| @@ -96,7 +96,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo ## obstacle_layer plugin -* ``: Name corresponding to the `nav2_costmap_2d::ObstacleLayer` plugin. This name gets defined in `plugin_names`, default value is `obstacle_layer` +* ``: Name corresponding to the `nav2_costmap_2d::ObstacleLayer` plugin. This name gets defined in `plugins`, default value is `obstacle_layer` * ``: Name of a source provided in ```.observation_sources` | Parameter | Default | Description | @@ -121,7 +121,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo ## range_sensor_layer plugin -* ``: Name corresponding to the `nav2_costmap_2d::RangeSensorLayer` plugin. This name gets defined in `plugin_names`. +* ``: Name corresponding to the `nav2_costmap_2d::RangeSensorLayer` plugin. This name gets defined in `plugins`. | Parameter | Default | Description | | ----------| --------| ------------| @@ -137,10 +137,10 @@ When `plugins` parameter is not overridden, the following default plugins are lo ## voxel_layer plugin -* ``: Name corresponding to the `nav2_costmap_2d::VoxelLayer` plugin. This name gets defined in `plugin_names` +* ``: Name corresponding to the `nav2_costmap_2d::VoxelLayer` plugin. This name gets defined in `plugins` * ``: Name of a source provided in ``.observation_sources` -*Note*: These parameters will only get declared if a `` name such as `voxel_layer` is appended to `plugin_names` parameter and `"nav2_costmap_2d::VoxelLayer"` is appended to `plugin_types` parameter. +*Note*: These parameters will only get declared if a `` name such as `voxel_layer` is appended to `plugins` parameter and `"nav2_costmap_2d::VoxelLayer"` is appended to its `plugin` name parameter. | Parameter | Default | Description | | ----------| --------| ------------| @@ -168,6 +168,15 @@ When `plugins` parameter is not overridden, the following default plugins are lo | ``.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap | | ``.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | +## keepout filter + +* ``: Name corresponding to the `nav2_costmap_2d::KeepoutFilter` plugin. This name gets defined in `plugins`. + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.enabled | true | Whether it is enabled | +| ``.filter_info_topic | N/A | Name of the CostmapFilterInfo topic having filter-related information | + # controller_server | Parameter | Default | Description | @@ -423,13 +432,14 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame # map_server -## map_server +## map_saver | Parameter | Default | Description | | ----------| --------| ------------| | save_map_timeout | 2000 | Timeout to attempt to save map with (ms) | | free_thresh_default | 0.25 | Free space maximum threshold for occupancy grid | | occupied_thresh_default | 0.65 | Occupied space minimum threshhold for occupancy grid | +| map_subscribe_transient_local | true | Use transient local QoS profile for incoming map subscription | ## map_server diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 4c9a2a93666..a639b2ee103 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -49,6 +49,7 @@ add_library(nav2_costmap_2d_core SHARED src/observation_buffer.cpp src/clear_costmap_service.cpp src/footprint_collision_checker.cpp + plugins/costmap_filters/costmap_filter.cpp ) # prevent pluginlib from using boost @@ -95,6 +96,16 @@ target_link_libraries(layers nav2_costmap_2d_core ) +add_library(filters SHARED + plugins/costmap_filters/keepout_filter.cpp +) +ament_target_dependencies(filters + ${dependencies} +) +target_link_libraries(filters + nav2_costmap_2d_core +) + add_library(nav2_costmap_2d_client SHARED src/footprint_subscriber.cpp src/costmap_subscriber.cpp @@ -131,11 +142,13 @@ ament_target_dependencies(nav2_costmap_2d target_link_libraries(nav2_costmap_2d nav2_costmap_2d_core layers + filters ) install(TARGETS nav2_costmap_2d_core layers + filters nav2_costmap_2d_client ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -169,7 +182,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(layers nav2_costmap_2d_core nav2_costmap_2d_client) +ament_export_libraries(layers filters nav2_costmap_2d_core nav2_costmap_2d_client) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_package() diff --git a/nav2_costmap_2d/README.md b/nav2_costmap_2d/README.md index 9dd12247e12..779398fd6b2 100644 --- a/nav2_costmap_2d/README.md +++ b/nav2_costmap_2d/README.md @@ -13,7 +13,7 @@ general ROS2 community. A proposal temporary replacement has been submitted as a ## How to configure using Voxel Layer plugin: By default, the navigation stack uses the _Obstacle Layer_ to avoid obstacles in 2D. The _Voxel Layer_ allows for detecting obstacles in 3D using Pointcloud2 data. It requires Pointcloud2 data being published on some topic. For simulation, a Gazebo model of the robot with depth camera enabled will publish a pointcloud topic. -The Voxel Layer plugin can be used to update the local costmap, glocal costmap or both, depending on how you define it in the `nav2_params.yaml` file in the nav2_bringup directory. The voxel layer plugin has to be added to the list of ```plugin_names``` and ```plugin_types``` in the global/local costmap scopes in the param file. If these are not defined in the param file, the default plugins set in nav2_costmap_2d will be used. +The Voxel Layer plugin can be used to update the local costmap, glocal costmap or both, depending on how you define it in the `nav2_params.yaml` file in the nav2_bringup directory. The voxel layer plugin has to be added to the list of ```plugins``` and its ```plugin``` type should be correctly specified in the global/local costmap scopes in the param file. If these are not defined in the param file, the default plugins set in nav2_costmap_2d will be used. Inside each costmap layer (voxel, obstacle, etc) define `observation_sources` param. Here you can define multiple sources to be used with the layer. The param configuration example below shows the way you can configure costmaps to use voxel layer. @@ -24,9 +24,9 @@ Example param configuration snippet for enabling voxel layer in local costmap is local_costmap: local_costmap: ros__parameters: - plugin_names: ["obstacle_layer", "voxel_layer", "inflation_layer"] - plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"] + plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan scan: @@ -36,6 +36,7 @@ local_costmap: marking: True data_type: "LaserScan" voxel_layer: + plugin: nav2_costmap_2d::VoxelLayer enabled: True publish_voxel_map: True origin_z: 0.0 @@ -74,9 +75,9 @@ For example, to add laser scan and pointcloud as two different sources of inputs local_costmap: local_costmap: ros__parameters: - plugin_names: ["obstacle_layer", "inflation_layer"] - plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"] + plugins: ["obstacle_layer", "inflation_layer"] obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan pointcloud scan: @@ -88,6 +89,12 @@ local_costmap: ``` In order to add multiple sources to the global costmap, follow the same procedure shown in the example above, but now adding the sources and their specific params under the `global_costmap` scope. +## Costmap Filters + +### Overview + +Costmap Filters - is a costmap layer-based instrument which provides an ability to apply to map spatial-dependent raster features named as filter-masks. These features are used in plugin algorithms when filling costmaps in order to allow robots to change their trajectory, behavior or speed when a robot enters/leaves an area marked in a filter masks. Examples of costmap filters include keep-out/safety zones where robots will never enter, speed restriction areas, preferred lanes for robots moving in industries and warehouses. More information about design, architecture of the feature and how it works could be found on navigation2 web-site: https://navigation.ros.org. + ## Future Plans - Conceptually, the costmap_2d model acts as a world model of what is known from the map, sensor, robot pose, etc. We'd like to broaden this world model concept and use costmap's layer concept as motivation for providing a service-style interface to diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index a0725392332..3adad664d3d 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -16,5 +16,11 @@ A range-sensor (sonar, IR) based obstacle layer for costmap_2d + + + + Prevents the robot from appearing in keepout zones marked on map. + + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 8f9fd438677..8e0f6f86480 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -48,6 +48,7 @@ #include #include #include "geometry_msgs/msg/point.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" namespace nav2_costmap_2d { @@ -87,6 +88,12 @@ class Costmap2D */ Costmap2D(const Costmap2D & map); + /** + * @brief Constructor for a costmap from an OccupancyGrid map + * @param map The OccupancyGrid map to create costmap from + */ + Costmap2D(const nav_msgs::msg::OccupancyGrid & map); + /** * @brief Overloaded assignment operator * @param map The costmap to copy 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 new file mode 100644 index 00000000000..16918ed1183 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -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 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 +#include + +#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_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp new file mode 100644 index 00000000000..07cc73ea91b --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -0,0 +1,82 @@ +/********************************************************************* + * + * 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 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__KEEPOUT_FILTER_HPP_ +#define NAV2_COSTMAP_2D__KEEPOUT_FILTER_HPP_ + +#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" + +namespace nav2_costmap_2d +{ + +class KeepoutFilter : public CostmapFilter +{ +public: + KeepoutFilter(); + + void initializeFilter( + const std::string & filter_info_topic); + + 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); + + void resetFilter(); + + bool isActive(); + +private: + void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg); + void maskCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + + rclcpp::Subscription::SharedPtr filter_info_sub_; + rclcpp::Subscription::SharedPtr mask_sub_; + + std::unique_ptr mask_costmap_; +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__KEEPOUT_FILTER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index e2ff924e3ce..4f7aa00ac0c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -126,6 +126,8 @@ class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface void declareParameter( const std::string & param_name, const rclcpp::ParameterValue & value); + void declareParameter( + const std::string & param_name); bool hasParameter(const std::string & param_name); void undeclareAllParameters(); std::string getFullName(const std::string & param_name); diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp new file mode 100644 index 00000000000..dec1437f33f --- /dev/null +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -0,0 +1,117 @@ +/********************************************************************* + * + * 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 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 + *********************************************************************/ + +#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" + +#include + +namespace nav2_costmap_2d +{ + +CostmapFilter::CostmapFilter() +: filter_info_topic_(""), mask_topic_("") +{ + access_ = new mutex_t(); +} + +CostmapFilter::~CostmapFilter() +{ + delete access_; +} + +void CostmapFilter::onInitialize() +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Declare parameters + declareParameter("enabled", rclcpp::ParameterValue(true)); + declareParameter("filter_info_topic"); + + // Get parameters + node->get_parameter(name_ + "." + "enabled", enabled_); + try { + filter_info_topic_ = node->get_parameter(name_ + "." + "filter_info_topic").as_string(); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR(node->get_logger(), "filter_info_topic parameter is not set"); + throw ex; + } +} + +void CostmapFilter::activate() +{ + initializeFilter(filter_info_topic_); +} + +void CostmapFilter::deactivate() +{ + resetFilter(); +} + +void CostmapFilter::reset() +{ + resetFilter(); + initializeFilter(filter_info_topic_); +} + +void CostmapFilter::updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * /*min_x*/, double * /*min_y*/, double * /*max_x*/, double * /*max_y*/) +{ + if (!enabled_) { + return; + } + + latest_pose_.x = robot_x; + latest_pose_.y = robot_y; + latest_pose_.theta = robot_yaw; +} + +void CostmapFilter::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j) +{ + if (!enabled_) { + return; + } + + process(master_grid, min_i, min_j, max_i, max_j, latest_pose_); +} + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp new file mode 100644 index 00000000000..b3a23ebc5dc --- /dev/null +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -0,0 +1,257 @@ +/********************************************************************* + * + * 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 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 + *********************************************************************/ + +#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" + +namespace nav2_costmap_2d +{ + +KeepoutFilter::KeepoutFilter() +: filter_info_sub_(nullptr), mask_sub_(nullptr), mask_costmap_(nullptr) +{ +} + +void KeepoutFilter::initializeFilter( + const std::string & filter_info_topic) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + filter_info_topic_ = filter_info_topic; + // Setting new costmap filter info subscriber + RCLCPP_INFO( + node->get_logger(), + "Subscribing to \"%s\" topic for filter info...", + filter_info_topic.c_str()); + filter_info_sub_ = node->create_subscription( + filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1)); +} + +void KeepoutFilter::filterInfoCallback( + const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Resetting previous subscriber each time when new costmap filter information arrives + if (mask_sub_) { + RCLCPP_WARN( + node->get_logger(), + "New costmap filter info arrived from %s topic. Updating old filter info.", + filter_info_topic_.c_str()); + mask_sub_.reset(); + } + + // Checking that base and multiplier are set to their default values + if (msg->base != BASE_DEFAULT or msg->multiplier != MULTIPLIER_DEFAULT) { + RCLCPP_ERROR( + node->get_logger(), + "For proper use of keepout filter base and multiplier in CostmapFilterInfo message " + "should be set to their default values (%f and %f)", + BASE_DEFAULT, MULTIPLIER_DEFAULT); + } + + mask_topic_ = msg->filter_mask_topic; + + // Setting new filter mask subscriber + RCLCPP_INFO( + node->get_logger(), + "Subscribing to \"%s\" topic for filter mask...", + msg->filter_mask_topic.c_str()); + mask_sub_ = node->create_subscription( + mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1)); +} + +void KeepoutFilter::maskCallback( + const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (mask_costmap_) { + RCLCPP_WARN( + node->get_logger(), + "New filter mask arrived from %s topic. Updating old filter mask.", + mask_topic_.c_str()); + mask_costmap_.reset(); + } + + // Making a new mask_costmap_ + mask_costmap_ = std::make_unique(*msg); +} + +void KeepoutFilter::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*/) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!mask_costmap_) { + // Show warning message every 2 seconds to not litter an output + RCLCPP_WARN_THROTTLE( + node->get_logger(), *(node->get_clock()), 2000, + "Filter mask was not received"); + return; + } + + double wx, wy; // world coordinates + + // Optimization: iterate only in overlapped + // (min_i, min_j)..(max_i, max_j) & mask_costmap_ area. + // + // mask_costmap_ + // *----------------------------* + // | | + // | | + // | (2) | + // *-----+-------* | + // | |///////|<- overlapped area | + // | |///////| to iterate in | + // | *-------+--------------------* + // | (1) | + // | | + // *-------------* + // master_grid (min_i, min_j)..(max_i, max_j) window + // + // ToDo: after costmap rotation will be added, this should be re-worked. + + // Calculating bounds corresponding to bottom-left overlapping (1) corner + int mg_min_x, mg_min_y; // masger_grid indexes of bottom-left (1) corner + // mask_costmap_ -> master_grid intexes conversion + const double half_cell_size = 0.5 * mask_costmap_->getResolution(); + wx = mask_costmap_->getOriginX() + half_cell_size; + wy = mask_costmap_->getOriginY() + half_cell_size; + master_grid.worldToMapNoBounds(wx, wy, mg_min_x, mg_min_y); + // Calculation of (1) corner bounds + if (mg_min_x >= max_i || mg_min_y >= max_j) { + // There is no overlapping. Do nothing. + return; + } + mg_min_x = std::max(min_i, mg_min_x); + mg_min_y = std::max(min_j, mg_min_y); + + // Calculating bounds corresponding to top-right window (2) corner + int mg_max_x, mg_max_y; // masger_grid indexes of top-right (2) corner + // mask_costmap_ -> master_grid intexes conversion + wx = mask_costmap_->getOriginX() + + mask_costmap_->getSizeInCellsX() * mask_costmap_->getResolution() + half_cell_size; + wy = mask_costmap_->getOriginY() + + mask_costmap_->getSizeInCellsY() * mask_costmap_->getResolution() + half_cell_size; + master_grid.worldToMapNoBounds(wx, wy, mg_max_x, mg_max_y); + // Calculation of (2) corner bounds + if (mg_max_x <= min_i || mg_max_y <= min_j) { + // There is no overlapping. Do nothing. + return; + } + mg_max_x = std::min(max_i, mg_max_x); + mg_max_y = std::min(max_j, mg_max_y); + + // unsigned<-signed conversions. + unsigned const int mg_min_x_u = static_cast(mg_min_x); + unsigned const int mg_min_y_u = static_cast(mg_min_y); + unsigned const int mg_max_x_u = static_cast(mg_max_x); + unsigned const int mg_max_y_u = static_cast(mg_max_y); + + unsigned int i, j; // master_grid iterators + unsigned int index; // corresponding index of master_grid + unsigned int mx, my; // mask_costmap_ coordinates + unsigned char data, old_data; // master_grid element data + + // Main master_grid updating loop + // Iterate in overlapped window by master_grid indexes + unsigned char * master_array = master_grid.getCharMap(); + for (i = mg_min_x_u; i < mg_max_x_u; i++) { + for (j = mg_min_y_u; j < mg_max_y_u; j++) { + index = master_grid.getIndex(i, j); + old_data = master_array[index]; + // Calculating corresponding to (i, j) point at mask_costmap_ + master_grid.mapToWorld(i, j, wx, wy); + if (mask_costmap_->worldToMap(wx, wy, mx, my)) { + data = mask_costmap_->getCost(mx, my); + // Update if mask_ data is valid and greater than existing master_grid's one + if (data == NO_INFORMATION) { + continue; + } + if (data > old_data || old_data == NO_INFORMATION) { + master_array[index] = data; + } + } + } + } +} + +void KeepoutFilter::resetFilter() +{ + std::lock_guard guard(*getMutex()); + + filter_info_sub_.reset(); + mask_sub_.reset(); +} + +bool KeepoutFilter::isActive() +{ + std::lock_guard guard(*getMutex()); + if (mask_costmap_) { + return true; + } + return false; +} + +} // namespace nav2_costmap_2d + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::KeepoutFilter, nav2_costmap_2d::Layer) diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index e5fc9472920..dff5dbbb0ea 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -41,6 +41,8 @@ #include #include #include +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/occ_grid_values.hpp" namespace nav2_costmap_2d { @@ -57,6 +59,37 @@ Costmap2D::Costmap2D( resetMaps(); } +Costmap2D::Costmap2D(const nav_msgs::msg::OccupancyGrid & map) +: default_value_(FREE_SPACE) +{ + access_ = new mutex_t(); + + // fill local variables + size_x_ = map.info.width; + size_y_ = map.info.height; + resolution_ = map.info.resolution; + origin_x_ = map.info.origin.position.x; + origin_y_ = map.info.origin.position.y; + + // create the costmap + costmap_ = new unsigned char[size_x_ * size_y_]; + + // fill the costmap with a data + int8_t data; + for (unsigned int it = 0; it < size_x_ * size_y_; it++) { + data = map.data[it]; + if (data == nav2_util::OCC_GRID_UNKNOWN) { + costmap_[it] = NO_INFORMATION; + } else { + // Linear conversion from OccupancyGrid data range [OCC_GRID_FREE..OCC_GRID_OCCUPIED] + // to costmap data range [FREE_SPACE..LETHAL_OBSTACLE] + costmap_[it] = std::round( + static_cast(data) * (LETHAL_OBSTACLE - FREE_SPACE) / + (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE)); + } + } +} + void Costmap2D::deleteMaps() { // clean up data diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index bdfe0e51164..aad00008a4f 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -89,6 +89,19 @@ Layer::declareParameter( node, getFullName(param_name), value); } +void +Layer::declareParameter( + const std::string & param_name) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + local_params_.insert(param_name); + nav2_util::declare_parameter_if_not_declared( + node, getFullName(param_name)); +} + bool Layer::hasParameter(const std::string & param_name) { diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 78085141b38..86b36cf3fb1 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -7,3 +7,19 @@ ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) target_link_libraries(collision_footprint_test nav2_costmap_2d_core ) + +ament_add_gtest(costmap_convesion_test costmap_conversion_test.cpp) +target_link_libraries(costmap_convesion_test + nav2_costmap_2d_core +) + +ament_add_gtest(declare_parameter_test declare_parameter_test.cpp) +target_link_libraries(declare_parameter_test + nav2_costmap_2d_core +) + +ament_add_gtest(keepout_filter_test keepout_filter_test.cpp) +target_link_libraries(keepout_filter_test + nav2_costmap_2d_core + filters +) diff --git a/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp b/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp new file mode 100644 index 00000000000..f0cf789a295 --- /dev/null +++ b/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp @@ -0,0 +1,122 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/occ_grid_values.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +static constexpr double EPSILON = std::numeric_limits::epsilon(); +static constexpr double RESOLUTION = 0.05; +static constexpr double ORIGIN_X = 0.1; +static constexpr double ORIGIN_Y = 0.2; + +class TestNode : public ::testing::Test +{ +public: + TestNode() {} + + ~TestNode() + { + occ_grid_.reset(); + costmap_.reset(); + } + +protected: + void createMaps(); + void verifyCostmap(); + +private: + std::shared_ptr occ_grid_; + std::shared_ptr costmap_; +}; + +void TestNode::createMaps() +{ + // Create occ_grid_ map + occ_grid_ = std::make_shared(); + + const unsigned int width = 4; + const unsigned int height = 3; + + occ_grid_->info.resolution = RESOLUTION; + occ_grid_->info.width = width; + occ_grid_->info.height = height; + occ_grid_->info.origin.position.x = ORIGIN_X; + occ_grid_->info.origin.position.y = ORIGIN_Y; + occ_grid_->info.origin.position.z = 0.0; + occ_grid_->info.origin.orientation.x = 0.0; + occ_grid_->info.origin.orientation.y = 0.0; + occ_grid_->info.origin.orientation.z = 0.0; + occ_grid_->info.origin.orientation.w = 1.0; + occ_grid_->data.resize(width * height); + + int8_t data; + for (unsigned int i = 0; i < width * height; i++) { + data = i * 10; + if (data <= nav2_util::OCC_GRID_OCCUPIED) { + occ_grid_->data[i] = data; + } else { + occ_grid_->data[i] = nav2_util::OCC_GRID_UNKNOWN; + } + } + + // Create costmap_ (convert OccupancyGrid -> to Costmap2D) + costmap_ = std::make_shared(*occ_grid_); +} + +void TestNode::verifyCostmap() +{ + // Verify Costmap2D info + EXPECT_NEAR(costmap_->getResolution(), RESOLUTION, EPSILON); + EXPECT_NEAR(costmap_->getOriginX(), ORIGIN_X, EPSILON); + EXPECT_NEAR(costmap_->getOriginY(), ORIGIN_Y, EPSILON); + + // Verify Costmap2D data + unsigned int it; + unsigned char data, data_ref; + for (it = 0; it < (costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY() - 1); it++) { + data = costmap_->getCharMap()[it]; + if (it != costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY() - 1) { + data_ref = std::round( + static_cast(nav2_costmap_2d::LETHAL_OBSTACLE - nav2_costmap_2d::FREE_SPACE) * it / + 10); + } else { + data_ref = nav2_costmap_2d::NO_INFORMATION; + } + EXPECT_EQ(data, data_ref); + } +} + +TEST_F(TestNode, convertOccGridToCostmap) +{ + createMaps(); + verifyCostmap(); +} diff --git a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp new file mode 100644 index 00000000000..dea1bb35505 --- /dev/null +++ b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp @@ -0,0 +1,94 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/layer.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +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) {} +}; + +TEST(DeclareParameter, useValidParameter2Args) +{ + LayerWrapper layer; + nav2_util::LifecycleNode::SharedPtr node = + std::make_shared("test_node"); + tf2_ros::Buffer tf(node->get_clock()); + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr); + + layer.declareParameter("test1", rclcpp::ParameterValue("test_val1")); + try { + std::string val = node->get_parameter("test_layer.test1").as_string(); + EXPECT_EQ(val, "test_val1"); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + FAIL() << "test_layer.test1 parameter is not set"; + } +} + +TEST(DeclareParameter, useValidParameter1Arg) +{ + LayerWrapper layer; + nav2_util::LifecycleNode::SharedPtr node = + std::make_shared("test_node"); + tf2_ros::Buffer tf(node->get_clock()); + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr); + + layer.declareParameter("test2"); + try { + node->set_parameter(rclcpp::Parameter("test_layer.test2", "test_val2")); + std::string val = node->get_parameter("test_layer.test2").as_string(); + EXPECT_EQ(val, "test_val2"); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + FAIL() << "test_layer.test2 parameter is not set"; + } +} + +TEST(DeclareParameter, useInvalidParameter1Arg) +{ + LayerWrapper layer; + nav2_util::LifecycleNode::SharedPtr node = + std::make_shared("test_node"); + tf2_ros::Buffer tf(node->get_clock()); + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr); + + layer.declareParameter("test3"); + try { + std::string val = node->get_parameter("test_layer.test3").as_string(); + FAIL() << "Incorrectly handling test_layer.test3 parameters which was not set"; + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + SUCCEED(); + } +} diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp new file mode 100644 index 00000000000..43ac60b7044 --- /dev/null +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -0,0 +1,401 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/occ_grid_values.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" + +using namespace std::chrono_literals; + +static const std::string FILTER_NAME = "keepout_filter"; +static const std::string INFO_TOPIC = "costmap_filter_info"; +static const std::string MASK_TOPIC = "mask"; + +class InfoPublisher : public rclcpp::Node +{ +public: + InfoPublisher(double base, double multiplier) + : Node("costmap_filter_info_pub") + { + publisher_ = this->create_publisher( + INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + msg->type = 0; + msg->filter_mask_topic = MASK_TOPIC; + msg->base = base; + msg->multiplier = multiplier; + + publisher_->publish(std::move(msg)); + } + + ~InfoPublisher() + { + publisher_.reset(); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; // InfoPublisher + +class MaskPublisher : public rclcpp::Node +{ +public: + MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) + : Node("mask_pub") + { + publisher_ = this->create_publisher( + MASK_TOPIC, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + publisher_->publish(mask); + } + + ~MaskPublisher() + { + publisher_.reset(); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; // MaskPublisher + +struct Point +{ + unsigned int x, y; +}; + +class TestNode : public ::testing::Test +{ +public: + TestNode() {} + + ~TestNode() {} + +protected: + void createMaps(unsigned char master_value, int8_t mask_value); + void publishMaps(); + void rePublishInfo(double base, double multiplier); + void rePublishMask(); + void waitSome(const std::chrono::nanoseconds & duration); + void createKeepoutFilter(); + void verifyMasterGrid(unsigned char free_value, unsigned char keepout_value); + void testStandardScenario(unsigned char free_value, unsigned char keepout_value); + void reset(); + + std::shared_ptr keepout_filter_; + std::shared_ptr master_grid_; + + std::vector keepout_points_; + +private: + nav2_util::LifecycleNode::SharedPtr node_; + + std::shared_ptr mask_; + + std::shared_ptr info_publisher_; + std::shared_ptr mask_publisher_; +}; + +void TestNode::createMaps(unsigned char master_value, int8_t mask_value) +{ + // Make map and mask put as follows: + // + // map (10,10) + // *----------------* + // | mask (6,6) | + // | *-----* | + // | |/////| | + // | |/////| | + // | *-----* | + // | (3,3) | + // *----------------* + // (0,0) + + const double resolution = 1.0; + + // Create master_grid_ + unsigned int width = 10; + unsigned int height = 10; + master_grid_ = std::make_shared( + width, height, resolution, 0.0, 0.0, master_value); + + // Create mask_ + width = 3; + height = 3; + mask_ = std::make_shared(); + mask_->info.resolution = resolution; + mask_->info.width = width; + mask_->info.height = height; + mask_->info.origin.position.x = 3.0; + mask_->info.origin.position.y = 3.0; + mask_->info.origin.position.z = 0.0; + mask_->info.origin.orientation.x = 0.0; + mask_->info.origin.orientation.y = 0.0; + mask_->info.origin.orientation.z = 0.0; + mask_->info.origin.orientation.w = 1.0; + mask_->data.resize(width * height, mask_value); +} + +void TestNode::publishMaps() +{ + info_publisher_ = std::make_shared(0.0, 1.0); + mask_publisher_ = std::make_shared(*mask_); +} + +void TestNode::rePublishInfo(double base, double multiplier) +{ + info_publisher_.reset(); + info_publisher_ = std::make_shared(base, multiplier); + // Allow both CostmapFilterInfo and filter mask subscribers + // to receive a new message + waitSome(500ms); +} + +void TestNode::rePublishMask() +{ + mask_publisher_.reset(); + mask_publisher_ = std::make_shared(*mask_); + // Allow filter mask subscriber to receive a new message + waitSome(500ms); +} + +void TestNode::waitSome(const std::chrono::nanoseconds & duration) +{ + rclcpp::Time start_time = node_->now(); + while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) { + rclcpp::spin_some(node_->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } +} + +void TestNode::createKeepoutFilter() +{ + node_ = std::make_shared("test_node"); + tf2_ros::Buffer tf(node_->get_clock()); + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + node_->declare_parameter( + FILTER_NAME + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); + node_->set_parameter( + rclcpp::Parameter(FILTER_NAME + ".filter_info_topic", INFO_TOPIC)); + + keepout_filter_ = std::make_shared(); + keepout_filter_->initialize(&layers, FILTER_NAME, &tf, node_, nullptr, nullptr); + keepout_filter_->initializeFilter(INFO_TOPIC); + + // Wait until mask will be received by KeepoutFilter + while (!keepout_filter_->isActive()) { + rclcpp::spin_some(node_->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } +} + +void TestNode::verifyMasterGrid(unsigned char free_value, unsigned char keepout_value) +{ + unsigned int x, y; + bool is_checked; + + for (y = 0; y < master_grid_->getSizeInCellsY(); y++) { + for (x = 0; x < master_grid_->getSizeInCellsX(); x++) { + is_checked = false; + for (std::vector::iterator it = keepout_points_.begin(); + it != keepout_points_.end(); it++) + { + if (x == it->x && y == it->y) { + EXPECT_EQ(master_grid_->getCost(x, y), keepout_value); + is_checked = true; + break; + } + } + if (!is_checked) { + EXPECT_EQ(master_grid_->getCost(x, y), free_value); + } + } + } +} + +void TestNode::testStandardScenario(unsigned char free_value, unsigned char keepout_value) +{ + geometry_msgs::msg::Pose2D pose; + // Intersection window: added 4 points + keepout_filter_->process(*master_grid_, 2, 2, 5, 5, pose); + keepout_points_.push_back(Point{3, 3}); + keepout_points_.push_back(Point{3, 4}); + keepout_points_.push_back(Point{4, 3}); + keepout_points_.push_back(Point{4, 4}); + verifyMasterGrid(free_value, keepout_value); + // Two windows outside on the horisontal/vertical edge: no new points added + keepout_filter_->process(*master_grid_, 3, 6, 5, 7, pose); + keepout_filter_->process(*master_grid_, 6, 3, 7, 5, pose); + verifyMasterGrid(free_value, keepout_value); + // Corner window: added 1 point + keepout_filter_->process(*master_grid_, 5, 5, 6, 6, pose); + keepout_points_.push_back(Point{5, 5}); + verifyMasterGrid(free_value, keepout_value); + // Outside windows: no new points added + keepout_filter_->process(*master_grid_, 0, 0, 2, 2, pose); + keepout_filter_->process(*master_grid_, 0, 7, 2, 9, pose); + keepout_filter_->process(*master_grid_, 7, 0, 9, 2, pose); + keepout_filter_->process(*master_grid_, 7, 7, 9, 9, pose); + verifyMasterGrid(free_value, keepout_value); +} + +void TestNode::reset() +{ + mask_.reset(); + master_grid_.reset(); + info_publisher_.reset(); + mask_publisher_.reset(); + keepout_filter_.reset(); + node_.reset(); + keepout_points_.clear(); +} + +TEST_F(TestNode, testFreeMasterLethalKeepout) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED); + publishMaps(); + createKeepoutFilter(); + + // Test KeepoutFilter + testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testUnknownMasterNonLethalKeepout) +{ + // Initilize test system + createMaps( + nav2_costmap_2d::NO_INFORMATION, + (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE) / 2); + publishMaps(); + createKeepoutFilter(); + + // Test KeepoutFilter + testStandardScenario( + nav2_costmap_2d::NO_INFORMATION, + (nav2_costmap_2d::LETHAL_OBSTACLE - nav2_costmap_2d::FREE_SPACE) / 2); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testFreeKeepout) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_FREE); + publishMaps(); + createKeepoutFilter(); + + // Test KeepoutFilter + geometry_msgs::msg::Pose2D pose; + // Check whole area window + keepout_filter_->process(*master_grid_, 0, 0, 10, 10, pose); + // There should be no one point appeared on master_grid_ after process() + verifyMasterGrid(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testUnknownKeepout) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_UNKNOWN); + publishMaps(); + createKeepoutFilter(); + + // Test KeepoutFilter + geometry_msgs::msg::Pose2D pose; + // Check whole area window + keepout_filter_->process(*master_grid_, 0, 0, 10, 10, pose); + // There should be no one point appeared on master_grid_ after process() + verifyMasterGrid(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testInfoRePublish) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED); + publishMaps(); + createKeepoutFilter(); + + // Re-publish filter info (with incorrect base and multiplier) + // and test that everything is working after + rePublishInfo(0.1, 0.2); + + // Test KeepoutFilter + testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testMaskRePublish) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED); + publishMaps(); + createKeepoutFilter(); + + // Re-publish filter mask and test that everything is working after + rePublishMask(); + + // Test KeepoutFilter + testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.jpg b/nav2_costmap_2d/test/unit/keepout_filter_test.jpg new file mode 100644 index 00000000000..18d21a118e9 Binary files /dev/null and b/nav2_costmap_2d/test/unit/keepout_filter_test.jpg differ diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 92437424863..644060d5b0f 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -25,6 +25,8 @@ set(map_saver_cli_executable map_saver_cli) set(map_saver_server_executable map_saver_server) +set(costmap_filter_info_server_executable costmap_filter_info_server) + add_executable(${map_server_executable} src/map_server/main.cpp) @@ -34,6 +36,9 @@ add_executable(${map_saver_cli_executable} add_executable(${map_saver_server_executable} src/map_saver/main_server.cpp) +add_executable(${costmap_filter_info_server_executable} + src/costmap_filter_info/main.cpp) + set(map_io_library_name map_io) set(library_name ${map_server_executable}_core) @@ -44,7 +49,8 @@ add_library(${map_io_library_name} SHARED add_library(${library_name} SHARED src/map_server/map_server.cpp - src/map_saver/map_saver.cpp) + src/map_saver/map_saver.cpp + src/costmap_filter_info/costmap_filter_info_server.cpp) set(map_io_dependencies yaml_cpp_vendor @@ -63,6 +69,7 @@ set(map_server_dependencies set(map_saver_dependencies rclcpp + rclcpp_lifecycle nav_msgs nav2_msgs nav2_util) @@ -76,6 +83,9 @@ ament_target_dependencies(${map_saver_cli_executable} ament_target_dependencies(${map_saver_server_executable} ${map_saver_dependencies}) +ament_target_dependencies(${costmap_filter_info_server_executable} + ${map_saver_dependencies}) + ament_target_dependencies(${library_name} ${map_server_dependencies}) @@ -99,6 +109,9 @@ target_link_libraries(${map_saver_cli_executable} target_link_libraries(${map_saver_server_executable} ${library_name}) +target_link_libraries(${costmap_filter_info_server_executable} + ${library_name}) + target_include_directories(${map_io_library_name} SYSTEM PRIVATE ${GRAPHICSMAGICKCPP_INCLUDE_DIRS}) @@ -118,6 +131,7 @@ install(TARGETS install(TARGETS ${map_server_executable} ${map_saver_cli_executable} ${map_saver_server_executable} + ${costmap_filter_info_server_executable} RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ diff --git a/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp b/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp new file mode 100644 index 00000000000..99311deff75 --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp @@ -0,0 +1,79 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_ +#define NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" + +namespace nav2_map_server +{ + +class CostmapFilterInfoServer : public nav2_util::LifecycleNode +{ +public: + /** + * @brief Constructor for the nav2_map_server::CostmapFilterInfoServer + */ + CostmapFilterInfoServer(); + /** + * @brief Destructor for the nav2_map_server::CostmapFilterInfoServer + */ + ~CostmapFilterInfoServer(); + +protected: + /** + * @brief Creates CostmapFilterInfo publisher and forms published message from ROS parameters + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief Publishes a CostmapFilterInfo message + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief Deactivates publisher + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief Resets publisher + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called when in Shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + +private: + rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; + + std::unique_ptr msg_; +}; // CostmapFilterInfoServer + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_ diff --git a/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp b/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp new file mode 100644 index 00000000000..4f1702089b2 --- /dev/null +++ b/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp @@ -0,0 +1,105 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// TODO(AlexeyMerzlyakov): This dummy info publisher should be removed +// after Semantic Map Server having the same functionality will be developed. + +#include "nav2_map_server/costmap_filter_info_server.hpp" + +#include +#include +#include + +namespace nav2_map_server +{ + +CostmapFilterInfoServer::CostmapFilterInfoServer() +: nav2_util::LifecycleNode("costmap_filter_info_server") +{ + declare_parameter("filter_info_topic", "costmap_filter_info"); + declare_parameter("type", 0); + declare_parameter("mask_topic", "filter_mask"); + declare_parameter("base", 0.0); + declare_parameter("multiplier", 1.0); +} + +CostmapFilterInfoServer::~CostmapFilterInfoServer() +{ +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + std::string filter_info_topic = get_parameter("filter_info_topic").as_string(); + + publisher_ = this->create_publisher( + filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + msg_ = std::make_unique(); + msg_->type = get_parameter("type").as_int(); + msg_->filter_mask_topic = get_parameter("mask_topic").as_string(); + msg_->base = get_parameter("base").as_double(); + msg_->multiplier = get_parameter("multiplier").as_double(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + publisher_->on_activate(); + publisher_->publish(std::move(msg_)); + + // create bond connection + createBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + publisher_->on_deactivate(); + + // destroy bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + publisher_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + + return nav2_util::CallbackReturn::SUCCESS; +} + +} // namespace nav2_map_server diff --git a/nav2_map_server/src/costmap_filter_info/main.cpp b/nav2_map_server/src/costmap_filter_info/main.cpp new file mode 100644 index 00000000000..e7b48f613e4 --- /dev/null +++ b/nav2_map_server/src/costmap_filter_info/main.cpp @@ -0,0 +1,32 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "nav2_map_server/costmap_filter_info_server.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char * argv[]) +{ + auto logger = rclcpp::get_logger("costmap_filter_info_server"); + + RCLCPP_INFO(logger, "This is costmap filter info publisher"); + + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 6941b354fe7..d926c5b07e1 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -46,6 +46,7 @@ #include "yaml-cpp/yaml.h" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/LinearMath/Quaternion.h" +#include "nav2_util/occ_grid_values.hpp" #ifdef _WIN32 // https://github.com/rtv/Stage/blob/master/replace/dirname.c @@ -213,20 +214,20 @@ void loadMapFromFile( switch (load_parameters.mode) { case MapMode::Trinary: if (load_parameters.occupied_thresh < occ) { - map_cell = 100; + map_cell = nav2_util::OCC_GRID_OCCUPIED; } else if (occ < load_parameters.free_thresh) { - map_cell = 0; + map_cell = nav2_util::OCC_GRID_FREE; } else { - map_cell = -1; + map_cell = nav2_util::OCC_GRID_UNKNOWN; } break; case MapMode::Scale: if (pixel.alphaQuantum() != OpaqueOpacity) { - map_cell = -1; + map_cell = nav2_util::OCC_GRID_UNKNOWN; } else if (load_parameters.occupied_thresh < occ) { - map_cell = 100; + map_cell = nav2_util::OCC_GRID_OCCUPIED; } else if (occ < load_parameters.free_thresh) { - map_cell = 0; + map_cell = nav2_util::OCC_GRID_FREE; } else { map_cell = std::rint( (occ - load_parameters.free_thresh) / @@ -235,10 +236,12 @@ void loadMapFromFile( break; case MapMode::Raw: { double occ_percent = std::round(shade * 255); - if (0 <= occ_percent && occ_percent <= 100) { + if (nav2_util::OCC_GRID_FREE <= occ_percent && + occ_percent <= nav2_util::OCC_GRID_OCCUPIED) + { map_cell = static_cast(occ_percent); } else { - map_cell = -1; + map_cell = nav2_util::OCC_GRID_UNKNOWN; } break; } diff --git a/nav2_map_server/test/unit/CMakeLists.txt b/nav2_map_server/test/unit/CMakeLists.txt index e0326d462c6..095a6509aef 100644 --- a/nav2_map_server/test/unit/CMakeLists.txt +++ b/nav2_map_server/test/unit/CMakeLists.txt @@ -9,3 +9,14 @@ target_link_libraries(test_map_io ${map_io_library_name} stdc++fs ) + +# costmap_filter_info_server unit test +ament_add_gtest(test_costmap_filter_info_server + test_costmap_filter_info_server.cpp +) + +ament_target_dependencies(test_costmap_filter_info_server rclcpp) + +target_link_libraries(test_costmap_filter_info_server + ${library_name} +) diff --git a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp new file mode 100644 index 00000000000..168fbc9f7b9 --- /dev/null +++ b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp @@ -0,0 +1,146 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "nav2_map_server/costmap_filter_info_server.hpp" + +using namespace std::chrono_literals; + +typedef std::recursive_mutex mutex_t; + +static const char FILTER_INFO_TOPIC[] = "filter_info"; +static const int TYPE = 1; +static const char MASK_TOPIC[] = "mask"; +static const double BASE = 0.1; +static const double MULTIPLIER = 0.2; + +static const double EPSILON = std::numeric_limits::epsilon(); + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class InfoServerWrapper : public nav2_map_server::CostmapFilterInfoServer +{ +public: + void start() + { + on_configure(get_current_state()); + on_activate(get_current_state()); + } + + void stop() + { + on_deactivate(get_current_state()); + on_cleanup(get_current_state()); + on_shutdown(get_current_state()); + } +}; + +class InfoServerTester : public ::testing::Test +{ +public: + InfoServerTester() + : info_server_(nullptr), info_(nullptr), subscription_(nullptr) + { + access_ = new mutex_t(); + + info_server_ = std::make_shared(); + try { + info_server_->set_parameter(rclcpp::Parameter("filter_info_topic", FILTER_INFO_TOPIC)); + info_server_->set_parameter(rclcpp::Parameter("type", TYPE)); + info_server_->set_parameter(rclcpp::Parameter("mask_topic", MASK_TOPIC)); + info_server_->set_parameter(rclcpp::Parameter("base", BASE)); + info_server_->set_parameter(rclcpp::Parameter("multiplier", MULTIPLIER)); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR( + info_server_->get_logger(), + "Error while setting parameters for CostmapFilterInfoServer: %s", ex.what()); + throw; + } + + info_server_->start(); + + subscription_ = info_server_->create_subscription( + FILTER_INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&InfoServerTester::infoCallback, this, std::placeholders::_1)); + } + + ~InfoServerTester() + { + info_server_->stop(); + info_server_.reset(); + subscription_.reset(); + } + + bool isReceived() + { + std::lock_guard guard(*getMutex()); + if (info_) { + return true; + } else { + return false; + } + } + + mutex_t * getMutex() + { + return access_; + } + +protected: + std::shared_ptr info_server_; + nav2_msgs::msg::CostmapFilterInfo::SharedPtr info_; + +private: + void infoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg) + { + std::lock_guard guard(*getMutex()); + info_ = msg; + } + + rclcpp::Subscription::SharedPtr subscription_; + + mutex_t * access_; +}; + +TEST_F(InfoServerTester, testCostmapFilterInfoPublish) +{ + rclcpp::Time start_time = info_server_->now(); + while (!isReceived()) { + rclcpp::spin_some(info_server_->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + // Waiting no more than 5 seconds + ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms)); + } + + // Checking received CostmapFilterInfo for consistency + EXPECT_EQ(info_->type, TYPE); + EXPECT_EQ(info_->filter_mask_topic, MASK_TOPIC); + EXPECT_NEAR(info_->base, BASE, EPSILON); + EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON); +} diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 1ecdf58b0f5..d09cafa74cf 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -15,6 +15,7 @@ nav2_package() rosidl_generate_interfaces(${PROJECT_NAME} "msg/Costmap.msg" "msg/CostmapMetaData.msg" + "msg/CostmapFilterInfo.msg" "msg/VoxelGrid.msg" "msg/BehaviorTreeStatusChange.msg" "msg/BehaviorTreeLog.msg" diff --git a/nav2_msgs/msg/CostmapFilterInfo.msg b/nav2_msgs/msg/CostmapFilterInfo.msg new file mode 100644 index 00000000000..48410794528 --- /dev/null +++ b/nav2_msgs/msg/CostmapFilterInfo.msg @@ -0,0 +1,11 @@ +std_msgs/Header header +# Type of plugin used (keepout filter, speed limit in m/s, speed limit in percent, etc...) +uint8 type +# Name of filter mask topic +string filter_mask_topic +# Multiplier base offset and multiplier coefficient for conversion of OccGrid. +# Used to convert OccupancyGrid data values to filter space values. +# data -> into some other number space: +# space = data * multiplier + base +float32 base +float32 multiplier diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 8d67f4a9785..99d03699726 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -59,6 +59,7 @@ if(BUILD_TESTING) add_subdirectory(src/recoveries/spin) add_subdirectory(src/recoveries/wait) add_subdirectory(src/recoveries/backup) + add_subdirectory(src/costmap_filters) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() diff --git a/nav2_system_tests/maps/filters_room.pgm b/nav2_system_tests/maps/filters_room.pgm new file mode 100644 index 00000000000..e183e4583eb Binary files /dev/null and b/nav2_system_tests/maps/filters_room.pgm differ diff --git a/nav2_system_tests/maps/filters_room.yaml b/nav2_system_tests/maps/filters_room.yaml new file mode 100644 index 00000000000..d07eba673f8 --- /dev/null +++ b/nav2_system_tests/maps/filters_room.yaml @@ -0,0 +1,6 @@ +image: filters_room.pgm +resolution: 0.1 +origin: [-5.0, -5.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 diff --git a/nav2_system_tests/maps/keepout_mask.pgm b/nav2_system_tests/maps/keepout_mask.pgm new file mode 100644 index 00000000000..517835f3c51 Binary files /dev/null and b/nav2_system_tests/maps/keepout_mask.pgm differ diff --git a/nav2_system_tests/maps/keepout_mask.yaml b/nav2_system_tests/maps/keepout_mask.yaml new file mode 100644 index 00000000000..3583a5d2e15 --- /dev/null +++ b/nav2_system_tests/maps/keepout_mask.yaml @@ -0,0 +1,6 @@ +image: keepout_mask.pgm +resolution: 0.1 +origin: [-5.0, -5.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 diff --git a/nav2_system_tests/params/keepout_params.yaml b/nav2_system_tests/params/keepout_params.yaml new file mode 100644 index 00000000000..4f0d34ff861 --- /dev/null +++ b/nav2_system_tests/params/keepout_params.yaml @@ -0,0 +1,7 @@ +costmap_filter_info_server: + ros__parameters: + filter_info_topic: "costmap_filter_info" + type: 0 + mask_topic: "filter_mask" + base: 0.0 + multiplier: 1.0 diff --git a/nav2_system_tests/src/costmap_filters/CMakeLists.txt b/nav2_system_tests/src/costmap_filters/CMakeLists.txt new file mode 100644 index 00000000000..3e8784703a7 --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/CMakeLists.txt @@ -0,0 +1,24 @@ +find_package(nav2_costmap_2d REQUIRED) + +set(test_keepout_filter_exec test_keepout_filter) + +ament_add_gtest_executable(${test_keepout_filter_exec} + test_keepout_filter.cpp filters_tester.cpp +) +ament_target_dependencies(${test_keepout_filter_exec} + ${dependencies} + nav2_costmap_2d +) + +ament_add_test(test_keepout_filter + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_filters_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 120 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/filters_room.yaml + TEST_MASK=${PROJECT_SOURCE_DIR}/maps/keepout_mask.yaml + FILTER_PARAMS=${PROJECT_SOURCE_DIR}/params/keepout_params.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) diff --git a/nav2_system_tests/src/costmap_filters/costmap_filters_tests_launch.py b/nav2_system_tests/src/costmap_filters/costmap_filters_tests_launch.py new file mode 100755 index 00000000000..cafdde40408 --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/costmap_filters_tests_launch.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2020 Samsung Research Russia +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess +import launch_ros.actions +from launch_testing.legacy import LaunchTestService + + +def main(argv=sys.argv[1:]): + lifecycle_manager_cmd = launch_ros.actions.Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[{'node_names': ['map_server', 'map_mask_server', + 'costmap_filter_info_server', 'filters_tester']}, + {'autostart': True}]) + + map_file = os.getenv('TEST_MAP') + map_server_cmd = launch_ros.actions.Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[{'yaml_filename': map_file}]) + + mapMaskFile = os.getenv('TEST_MASK') + map_mask_server_cmd = launch_ros.actions.Node( + package='nav2_map_server', + executable='map_server', + name='map_mask_server', + output='screen', + parameters=[{'yaml_filename': mapMaskFile}, + {'topic_name': 'filter_mask'}]) + + filterParamsFile = os.getenv('FILTER_PARAMS') + costmap_filter_info_server_cmd = launch_ros.actions.Node( + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', + output='screen', + parameters=[filterParamsFile]) + + testExecutable = os.getenv('TEST_EXECUTABLE') + test_action = ExecuteProcess( + cmd=[testExecutable], + name='costmap_tests', + output='screen' + ) + + ld = LaunchDescription() + + # Add map server running + ld.add_action(lifecycle_manager_cmd) + ld.add_action(map_server_cmd) + ld.add_action(map_mask_server_cmd) + ld.add_action(costmap_filter_info_server_cmd) + + lts = LaunchTestService() + lts.add_test_action(ld, test_action) + + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/costmap_filters/filters_tester.cpp b/nav2_system_tests/src/costmap_filters/filters_tester.cpp new file mode 100644 index 00000000000..5f547a9dafa --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/filters_tester.cpp @@ -0,0 +1,310 @@ +// Copyright (c) 2020 Samsung Research Russia +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include "filters_tester.hpp" + +#include +#include +#include +#include +#include + +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_map_server/map_io.hpp" + +using namespace std::chrono_literals; + +namespace nav2_system_tests +{ + +FiltersTester::FiltersTester() +: nav2_util::LifecycleNode("filters_tester"), is_active_(false) +{ + RCLCPP_INFO(get_logger(), "Creating"); + costmap_ros_ = std::make_shared( + "costmap_2d_ros", "/", ""); + costmap_ros_->set_parameter(rclcpp::Parameter("always_send_full_costmap", true)); + costmap_ros_->set_parameter(rclcpp::Parameter("resolution", 0.1)); + costmap_ros_->set_parameter(rclcpp::Parameter("publish_frequency", 5.0)); + costmap_ros_->set_parameter(rclcpp::Parameter("update_frequency", 5.0)); + costmap_ros_->set_parameter(rclcpp::Parameter("robot_radius", 0.2)); + + std::vector plugins_list{"static_layer", "inflation_layer", "keepout_filter"}; + costmap_ros_->set_parameter(rclcpp::Parameter("plugins", plugins_list)); + // Since plugins are not initialized yet, plugins' parameters are not declared as well. + // We need to declare them before setting. + costmap_ros_->declare_parameter( + "static_layer.plugin", rclcpp::ParameterValue("nav2_costmap_2d::StaticLayer")); + costmap_ros_->set_parameter( + rclcpp::Parameter( + "static_layer.plugin", "nav2_costmap_2d::StaticLayer")); + costmap_ros_->declare_parameter( + "inflation_layer.plugin", rclcpp::ParameterValue("nav2_costmap_2d::InflationLayer")); + costmap_ros_->set_parameter( + rclcpp::Parameter( + "inflation_layer.plugin", "nav2_costmap_2d::InflationLayer")); + costmap_ros_->declare_parameter( + "keepout_filter.plugin", rclcpp::ParameterValue("nav2_costmap_2d::KeepoutFilter")); + costmap_ros_->set_parameter( + rclcpp::Parameter( + "keepout_filter.plugin", "nav2_costmap_2d::KeepoutFilter")); + costmap_ros_->declare_parameter( + "keepout_filter.filter_info_topic", rclcpp::ParameterValue("costmap_filter_info")); + costmap_ros_->set_parameter( + rclcpp::Parameter("keepout_filter.filter_info_topic", "costmap_filter_info")); + + planner_ = std::make_shared(); +} + +FiltersTester::~FiltersTester() +{ + RCLCPP_INFO(get_logger(), "Destroying"); +} + +nav2_util::CallbackReturn +FiltersTester::on_configure(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + costmap_ros_->on_configure(state); + + auto node = shared_from_this(); + auto tf = costmap_ros_->getTfBuffer(); + planner_->configure(node, "test_planner", tf, costmap_ros_); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +FiltersTester::on_activate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + startRobotTransform(); + + costmap_ros_->on_activate(state); + planner_->activate(); + + // Loading filter mask + nav_msgs::msg::OccupancyGrid mask_msg; + char * test_mask = std::getenv("TEST_MASK"); + nav2_map_server::LOAD_MAP_STATUS status = nav2_map_server::loadMapFromYaml(test_mask, mask_msg); + if (status != nav2_map_server::LOAD_MAP_SUCCESS) { + return nav2_util::CallbackReturn::FAILURE; + } + mask_costmap_ = std::make_unique(mask_msg); + + is_active_ = true; + + // create bond connection + createBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +FiltersTester::on_deactivate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + is_active_ = false; + + transform_timer_.reset(); + tf_broadcaster_.reset(); + + costmap_ros_->on_deactivate(state); + planner_->deactivate(); + + // destroy bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +FiltersTester::on_cleanup(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + costmap_ros_->on_cleanup(state); + planner_->cleanup(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +FiltersTester::on_error(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +FiltersTester::on_shutdown(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + + costmap_ros_->on_shutdown(state); + + return nav2_util::CallbackReturn::SUCCESS; +} + +void FiltersTester::startRobotTransform() +{ + // Provide the robot pose transform + tf_broadcaster_ = std::make_shared(this); + + // Set an initial pose + geometry_msgs::msg::Point robot_position; + robot_position.x = 0.0; + robot_position.y = 0.0; + updateRobotPosition(robot_position); + + // Publish the transform periodically + transform_timer_ = create_wall_timer( + 100ms, std::bind(&FiltersTester::publishRobotTransform, this)); +} + +void FiltersTester::updateRobotPosition(const geometry_msgs::msg::Point & position) +{ + if (!base_transform_) { + base_transform_ = std::make_unique(); + base_transform_->header.frame_id = "map"; + base_transform_->child_frame_id = "base_link"; + } + + base_transform_->header.stamp = now() + rclcpp::Duration(250000000); + base_transform_->transform.translation.x = position.x; + base_transform_->transform.translation.y = position.y; + base_transform_->transform.rotation.w = 1.0; + + publishRobotTransform(); +} + +void FiltersTester::publishRobotTransform() +{ + if (base_transform_) { + tf_broadcaster_->sendTransform(*base_transform_); + } +} + +void FiltersTester::spinTester() +{ + if (rclcpp::ok()) { + // Spin LifycycleNode and Costmap2DROS + rclcpp::spin_some(this->get_node_base_interface()); + rclcpp::spin_some(costmap_ros_->get_node_base_interface()); + } +} + +void FiltersTester::waitSome(const std::chrono::nanoseconds & duration) +{ + rclcpp::Time start_time = this->now(); + while (this->now() - start_time <= rclcpp::Duration(duration)) { + spinTester(); + std::this_thread::sleep_for(100ms); + } +} + +TestStatus FiltersTester::testPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & end) +{ + nav_msgs::msg::Path path; + + if (!is_active_) { + RCLCPP_WARN(get_logger(), "FiltersTester node is not activated yet"); + return NOT_ACTIVE; + } + + // Allow keepout_filter to receive CostmapFilterInfo and filter mask + waitSome(1000ms); + // After keepout_filter received the mask, force static layer to update bounds + costmap_ros_->resetLayers(); + waitSome(1000ms); + + if (!checkPlan(start, end, path)) { + // Fail case: can not produce the path to the goal + return NO_PATH; + } + // printPath(path); + for (unsigned int i = 0; i < path.poses.size(); i++) { + if (isInKeepout(path.poses[i].pose.position)) { + // Fail case: robot enters keepout area + return IN_KEEPOUT; + } + } + + return SUCCESS; +} + +bool FiltersTester::isInKeepout(const geometry_msgs::msg::Point & position) +{ + const double & x = position.x; + const double & y = position.y; + unsigned int mx, my; + + if (!mask_costmap_) { + RCLCPP_ERROR(get_logger(), "Filter mask was not loaded"); + return true; + } + + if (!mask_costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_ERROR(get_logger(), "Robot is out of world"); + return true; + } + + if (mask_costmap_->getCost(mx, my) == nav2_costmap_2d::LETHAL_OBSTACLE) { + RCLCPP_ERROR(get_logger(), "Position (%f,%f) belongs to keepout area (%i,%i)", x, y, mx, my); + return true; + } + + // Robot is not in the keepout area + return false; +} + +bool FiltersTester::checkPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & end, + nav_msgs::msg::Path & path) const +{ + try { + path = planner_->createPlan(start, end); + if (!path.poses.size()) { + return false; + } + } catch (...) { + return false; + } + return true; +} + +void FiltersTester::printPath(const nav_msgs::msg::Path & path) const +{ + auto index = 0; + auto ss = std::stringstream{}; + + ss << '\n'; + for (auto pose : path.poses) { + ss << " point #" << index << " with" << + " x: " << std::setprecision(3) << pose.pose.position.x << + " y: " << std::setprecision(3) << pose.pose.position.y << '\n'; + ++index; + } + + RCLCPP_INFO(get_logger(), ss.str().c_str()); +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/costmap_filters/filters_tester.hpp b/nav2_system_tests/src/costmap_filters/filters_tester.hpp new file mode 100644 index 00000000000..22500fcf2a2 --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/filters_tester.hpp @@ -0,0 +1,94 @@ +// Copyright (c) 2020 Samsung Research Russia +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef COSTMAP_FILTERS__FILTERS_TESTER_HPP_ +#define COSTMAP_FILTERS__FILTERS_TESTER_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_navfn_planner/navfn_planner.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +namespace nav2_system_tests +{ + +enum TestStatus {SUCCESS, NO_PATH, IN_KEEPOUT, NOT_ACTIVE}; + +class FiltersTester : public nav2_util::LifecycleNode +{ +public: + FiltersTester(); + ~FiltersTester(); + + // Test that planner can produce a path from start to end point + TestStatus testPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & end); + + // FiltersTester spinner + void spinTester(); + // Spin FilterTester for some time + void waitSome(const std::chrono::nanoseconds & duration); + + // Returns true if FiltersTester was activated + inline bool isActive() const + { + return is_active_; + } + +protected: + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; + +private: + std::shared_ptr costmap_ros_; + std::shared_ptr planner_; + + // The tester must provide the robot pose through a transform + std::unique_ptr base_transform_; + std::shared_ptr tf_broadcaster_; + std::unique_ptr mask_costmap_; + rclcpp::TimerBase::SharedPtr transform_timer_; + void startRobotTransform(); + void updateRobotPosition(const geometry_msgs::msg::Point & position); + void publishRobotTransform(); + + // Returns true if position is inside keepout zone + bool isInKeepout(const geometry_msgs::msg::Point & position); + + // Check that planner could make a plan from start to end point + bool checkPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & end, + nav_msgs::msg::Path & path) const; + + // Print produced plan. Debug + void printPath(const nav_msgs::msg::Path & path) const; + + bool is_active_; +}; + +} // namespace nav2_system_tests + +#endif // COSTMAP_FILTERS__FILTERS_TESTER_HPP_ diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_filter.cpp b/nav2_system_tests/src/costmap_filters/test_keepout_filter.cpp new file mode 100644 index 00000000000..9480e003fab --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/test_keepout_filter.cpp @@ -0,0 +1,75 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "filters_tester.hpp" + +using namespace std::chrono_literals; + +std::shared_ptr filters_tester; + +// Checks that roobt can produce valid path plan and reach the goal. +// The goal lies behind keepout area that should force +// path planner to re-planm so finally the goal should be reached. +TEST(TestKeepoutFilter, testValidPlan) +{ + geometry_msgs::msg::PoseStamped start; + start.pose.position.x = -4.0; + start.pose.position.y = -4.0; + geometry_msgs::msg::PoseStamped end; + end.pose.position.x = 4.0; + end.pose.position.y = 4.0; + + nav2_system_tests::TestStatus test_result = filters_tester->testPlan(start, end); + EXPECT_EQ(nav2_system_tests::SUCCESS, test_result); +} + +// Checks that robot will fail to go inside a keepout area +TEST(TestKeepoutFilter, testInvalidPlan) +{ + geometry_msgs::msg::PoseStamped start; + start.pose.position.x = -4.0; + start.pose.position.y = -4.0; + geometry_msgs::msg::PoseStamped end; + end.pose.position.x = 4.0; + end.pose.position.y = -4.0; + + nav2_system_tests::TestStatus test_result = filters_tester->testPlan(start, end); + EXPECT_EQ(nav2_system_tests::NO_PATH, test_result); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Make FiltersTester and wait until it will be active + filters_tester = std::make_shared(); + while (!filters_tester->isActive()) { + filters_tester->spinTester(); + } + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + filters_tester.reset(); + + return test_result; +} diff --git a/nav2_util/include/nav2_util/occ_grid_values.hpp b/nav2_util/include/nav2_util/occ_grid_values.hpp new file mode 100644 index 00000000000..d366c0c3c73 --- /dev/null +++ b/nav2_util/include/nav2_util/occ_grid_values.hpp @@ -0,0 +1,50 @@ +// Copyright (c) 2020 Samsung Research Russia +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 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_UTIL__OCC_GRID_VALUES_HPP_ +#define NAV2_UTIL__OCC_GRID_VALUES_HPP_ + +namespace nav2_util +{ + +/** + * @brief OccupancyGrid data constants + */ +static constexpr int8_t OCC_GRID_UNKNOWN = -1; +static constexpr int8_t OCC_GRID_FREE = 0; +static constexpr int8_t OCC_GRID_OCCUPIED = 100; + +} // namespace nav2_util + +#endif // NAV2_UTIL__OCC_GRID_VALUES_HPP_