Initial commit

nav2_costmap_filters_demo/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_costmap_filters_demo)
+
+set(CMAKE_CXX_STANDARD 14)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(DIRECTORY maps
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(DIRECTORY params
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

nav2_costmap_filters_demo/launch/costmap_filter_info.launch.py

@@ -0,0 +1,118 @@
+#!/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
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    # Get the launch directory
+    costmap_filters_demo_dir = get_package_share_directory('nav2_costmap_filters_demo')
+
+    # Create our own temporary YAML files that include substitutions
+    lifecycle_nodes = ['filter_mask_server', 'costmap_filter_info_server']
+
+    # Parameters
+    namespace = LaunchConfiguration('namespace')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    autostart = LaunchConfiguration('autostart')
+    params_file = LaunchConfiguration('params_file')
+    mask_yaml_file = LaunchConfiguration('mask')
+
+    # Declare the launch arguments
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Top-level namespace')
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='true',
+        description='Use simulation (Gazebo) clock if true')
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        'autostart', default_value='true',
+        description='Automatically startup the nav2 stack')
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+            'params_file',
+            description='Full path to the ROS2 parameters file to use')
+
+    declare_mask_yaml_file_cmd = DeclareLaunchArgument(
+            'mask',
+            description='Full path to filter mask yaml file to load')
+
+    # Make re-written yaml
+    param_substitutions = {
+        'use_sim_time': use_sim_time,
+        'yaml_filename': mask_yaml_file}
+
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key=namespace,
+        param_rewrites=param_substitutions,
+        convert_types=True)
+
+    # Nodes launching commands
+    start_lifecycle_manager_cmd = Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            name='lifecycle_manager_costmap_filters',
+            namespace=namespace,
+            output='screen',
+            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+            parameters=[{'use_sim_time': use_sim_time},
+                        {'autostart': autostart},
+                        {'node_names': lifecycle_nodes}])
+
+    start_map_server_cmd = Node(
+            package='nav2_map_server',
+            executable='map_server',
+            name='filter_mask_server',
+            namespace=namespace,
+            output='screen',
+            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+            parameters=[configured_params])
+
+    start_costmap_filter_info_server_cmd = Node(
+            package='nav2_map_server',
+            executable='costmap_filter_info_server',
+            name='costmap_filter_info_server',
+            namespace=namespace,
+            output='screen',
+            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+            parameters=[configured_params])
+
+    ld = LaunchDescription()
+
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_mask_yaml_file_cmd)
+
+    ld.add_action(start_lifecycle_manager_cmd)
+    ld.add_action(start_map_server_cmd)
+    ld.add_action(start_costmap_filter_info_server_cmd)
+
+    return ld

nav2_costmap_filters_demo/maps/keepout_mask.yaml

@@ -0,0 +1,7 @@
+image: keepout_mask.pgm
+mode: scale
+resolution: 0.100000
+origin: [-0.450000, -0.450000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

nav2_costmap_filters_demo/maps/speed_mask.yaml

@@ -0,0 +1,7 @@
+image: speed_mask.pgm
+mode: scale
+resolution: 0.050000
+origin: [-10.000000, -10.000000, 0.000000]
+negate: 0
+occupied_thresh: 1.0
+free_thresh: 0.0

nav2_costmap_filters_demo/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_costmap_filters_demo</name>
+  <version>1.0.0</version>
+  <description>Costmap Filters demo</description>
+  <maintainer email="[email protected]">Alexey Merzlyakov</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

nav2_costmap_filters_demo/params/keepout_params.yaml

@@ -0,0 +1,14 @@
+costmap_filter_info_server:
+  ros__parameters:
+    use_sim_time: true
+    type: 0
+    filter_info_topic: "/costmap_filter_info"
+    mask_topic: "/keepout_filter_mask"
+    base: 0.0
+    multiplier: 1.0
+filter_mask_server:
+  ros__parameters:
+    use_sim_time: true
+    frame_id: "map"
+    topic_name: "/keepout_filter_mask"
+    yaml_filename: "keepout_mask.yaml"

nav2_costmap_filters_demo/params/speed_params.yaml

@@ -0,0 +1,14 @@
+costmap_filter_info_server:
+  ros__parameters:
+    use_sim_time: true
+    type: 1
+    filter_info_topic: "/costmap_filter_info"
+    mask_topic: "/speed_filter_mask"
+    base: 100.0
+    multiplier: -1.0
+filter_mask_server:
+  ros__parameters:
+    use_sim_time: true
+    frame_id: "map"
+    topic_name: "/speed_filter_mask"
+    yaml_filename: "speed_mask.yaml"

nav2_gradient_costmap_plugin/CMakeLists.txt

@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_gradient_costmap_plugin)
+
+set(lib_name ${PROJECT_NAME}_core)
+
+# === Environment ===
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# === Dependencies ===
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(pluginlib REQUIRED)
+
+set(dep_pkgs
+    rclcpp
+    nav2_costmap_2d
+    pluginlib)
+
+# === Build ===
+
+add_library(${lib_name} SHARED
+            src/gradient_layer.cpp)
+include_directories(include)
+
+# === Installation ===
+
+install(TARGETS ${lib_name}
+        DESTINATION lib)
+
+# === Ament work ===
+
+# pluginlib_export_plugin_description_file() installs gradient_layer.xml
+# file into "share" directory and sets ament indexes for it.
+# This allows the plugin to be discovered as a plugin of required type.
+pluginlib_export_plugin_description_file(nav2_costmap_2d gradient_layer.xml)
+ament_target_dependencies(${lib_name} ${dep_pkgs})
+ament_package()

nav2_gradient_costmap_plugin/gradient_layer.xml

@@ -0,0 +1,5 @@
+<library path="nav2_gradient_costmap_plugin_core">
+  <class name="nav2_gradient_costmap_plugin/GradientLayer" type="nav2_gradient_costmap_plugin::GradientLayer" base_class_type="nav2_costmap_2d::Layer">
+    <description>This is an example plugin which puts repeating costs gradients to costmap</description>
+  </class>
+</library>

nav2_gradient_costmap_plugin/include/nav2_gradient_costmap_plugin/gradient_layer.hpp

@@ -0,0 +1,91 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  Copyright (c) 2020, Samsung R&D Institute 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 Willow Garage, Inc. 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: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *         Alexey Merzlyakov
+ *
+ * Reference tutorial:
+ * https://navigation.ros.org/tutorials/docs/writing_new_costmap2d_plugin.html
+ *********************************************************************/
+#ifndef GRADIENT_LAYER_HPP_
+#define GRADIENT_LAYER_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/layer.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+
+namespace nav2_gradient_costmap_plugin
+{
+
+class GradientLayer : public nav2_costmap_2d::Layer
+{
+public:
+  GradientLayer();
+
+  virtual void onInitialize();
+  virtual void updateBounds(
+    double robot_x, double robot_y, double robot_yaw, double * min_x,
+    double * min_y,
+    double * max_x,
+    double * max_y);
+  virtual void updateCosts(
+    nav2_costmap_2d::Costmap2D & master_grid,
+    int min_i, int min_j, int max_i, int max_j);
+
+  virtual void reset()
+  {
+    return;
+  }
+
+  virtual void onFootprintChanged();
+
+  virtual bool isClearable() {return false;}
+
+private:
+  double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
+
+  // Indicates that the entire gradient should be recalculated next time.
+  bool need_recalculation_;
+
+  // Size of gradient in cells
+  int GRADIENT_SIZE = 20;
+  // Step of increasing cost per one cell in gradient
+  int GRADIENT_FACTOR = 10;
+};
+
+}  // namespace nav2_gradient_costmap_plugin
+
+#endif  // GRADIENT_LAYER_HPP_

nav2_gradient_costmap_plugin/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_gradient_costmap_plugin</name>
+  <version>1.0.0</version>
+  <description>Run-time plugin for Costmap2D gradient later</description>
+  <maintainer email="[email protected]">Alexey Merzlyakov</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>nav2_costmap_2d</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+
+  <export>
+    <costmap_2d plugin="${prefix}/gradient_layer.xml" />
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>