From 5add7b8465a4ac71c01ad68caabf553515b0947c Mon Sep 17 00:00:00 2001 From: RyuYamamoto Date: Sat, 27 Apr 2024 16:28:19 +0900 Subject: [PATCH] feat: add costmap 2d --- navyu_costmap_2d/CMakeLists.txt | 37 +++++++++ .../config/navyu_costmap_2d_params.yaml | 11 +++ .../navyu_costmap_2d/navyu_costmap_2d.hpp | 44 ++++++++++ .../plugins/inflation_layer.hpp | 77 ++++++++++++++++++ .../navyu_costmap_2d/plugins/layer.hpp | 23 ++++++ .../navyu_costmap_2d/plugins/static_layer.hpp | 40 +++++++++ .../launch/navyu_costmap_2d.launch.py | 58 +++++++++++++ navyu_costmap_2d/package.xml | 28 +++++++ navyu_costmap_2d/src/navyu_costmap_2d.cpp | 81 +++++++++++++++++++ 9 files changed, 399 insertions(+) create mode 100644 navyu_costmap_2d/CMakeLists.txt create mode 100644 navyu_costmap_2d/config/navyu_costmap_2d_params.yaml create mode 100644 navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp create mode 100644 navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp create mode 100644 navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp create mode 100644 navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp create mode 100644 navyu_costmap_2d/launch/navyu_costmap_2d.launch.py create mode 100644 navyu_costmap_2d/package.xml create mode 100644 navyu_costmap_2d/src/navyu_costmap_2d.cpp diff --git a/navyu_costmap_2d/CMakeLists.txt b/navyu_costmap_2d/CMakeLists.txt new file mode 100644 index 0000000..9342c23 --- /dev/null +++ b/navyu_costmap_2d/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.5) +project(navyu_costmap_2d) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic ) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/${PROJECT_NAME}.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "NavyuCostmap2D" + EXECUTABLE ${PROJECT_NAME}_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config + rviz +) + +install(FILES DESTINATION share/${PROJECT_NAME}) diff --git a/navyu_costmap_2d/config/navyu_costmap_2d_params.yaml b/navyu_costmap_2d/config/navyu_costmap_2d_params.yaml new file mode 100644 index 0000000..958aede --- /dev/null +++ b/navyu_costmap_2d/config/navyu_costmap_2d_params.yaml @@ -0,0 +1,11 @@ +global_costmap_node: + ros__parameters: + update_frequency: 1.0 + base_frame_id: "base_link" + map_frame_id: "map" + plugins: ["static_layer", "inflation_layer"] + inflation_layer: + inflation_radius: 0.55 + robot_radius: 0.22 + static_layer: + map_topic: "map" diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp new file mode 100644 index 0000000..ccf46ec --- /dev/null +++ b/navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp @@ -0,0 +1,44 @@ +#ifndef NAVYU_COSTMAP_2D__NAVYU_COSTMAP_2D_HPP_ +#define NAVYU_COSTMAP_2D__NAVYU_COSTMAP_2D_HPP_ + +#include "navyu_costmap_2d/plugins/layer.hpp" + +#include + +#include +#include + +#include +#include +#include +#include + +class NavyuCostmap2D : public rclcpp::Node +{ +public: + explicit NavyuCostmap2D(const rclcpp::NodeOptions & node_options); + ~NavyuCostmap2D(); + + void update(); + +private: + geometry_msgs::msg::TransformStamped get_transform( + const std::string target_frame, const std::string source_frame); + +private: + rclcpp::Publisher::SharedPtr costmap_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + + std::vector plugins_; + std::map> layer_function_; + + tf2_ros::Buffer tf_buffer_{get_clock()}; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + std::shared_ptr broadcaster_; + + double update_frequency_; + std::string base_frame_id_; + std::string map_frame_id_; +}; + +#endif // NAVYU_COSTMAP_2D__NAVYU_COSTMAP_2D_HPP_ diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp new file mode 100644 index 0000000..5bb46b3 --- /dev/null +++ b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp @@ -0,0 +1,77 @@ +#ifndef NAVYU_COSTMAP_2D__PLUGINS__INFLATION_LAYER_HPP_ +#define NAVYU_COSTMAP_2D__PLUGINS__INFLATION_LAYER_HPP_ + +#include "navyu_costmap_2d/plugins/layer.hpp" + +class InflationLayer : public Layer +{ +public: + explicit InflationLayer(rclcpp::Node * node) : Layer(node) { node_ = node; } + + void initialize() override + { + node_->declare_parameter("inflation_layer.inflation_radius"); + node_->get_parameter("inflation_layer.inflation_radius", inflation_radius_); + node_->declare_parameter("inflation_layer.robot_radius"); + node_->get_parameter("inflation_layer.robot_radius", robot_radius_); + } + + void update(nav_msgs::msg::OccupancyGrid::SharedPtr & master_costmap) override + { + if (master_costmap == nullptr) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "costmap is nullptr."); + return; + } + + const int size_x = master_costmap->info.width; + const int size_y = master_costmap->info.height; + + for (int map_y = 0; map_y < size_y; map_y++) { + for (int map_x = 0; map_x < size_x; map_x++) { + int map_index = map_x + size_x * map_y; + + if (master_costmap->data[map_index] == LETHAL_COST) { + expand_cost(master_costmap, map_x, map_y); + } + } + } + } + + void expand_cost(nav_msgs::msg::OccupancyGrid::SharedPtr & costmap, const int mx, const int my) + { + const int size_x = costmap->info.width; + const double resolution = costmap->info.resolution; + const int cell_inflation_radius = std::ceil(inflation_radius_ / resolution); + + const int min_x = mx - cell_inflation_radius; + const int max_x = mx + cell_inflation_radius; + const int min_y = my - cell_inflation_radius; + const int max_y = my + cell_inflation_radius; + + for (int y = min_y; y < max_y; y++) { + for (int x = min_x; x < max_x; x++) { + const double distance = std::hypot(x - mx, y - my); + const int index = x + size_x * y; + const auto old_cost = costmap->data[index]; + + if (distance * resolution < robot_radius_) { + costmap->data[index] = std::max(INSCRIBED_COST, old_cost); + } else { + if (cell_inflation_radius < distance) continue; + if (costmap->data[index] == -1) continue; + const int8_t new_cost = + std::exp(-1 * 3.0 * (distance * resolution - robot_radius_)) * INSCRIBED_COST - 1; + costmap->data[index] = std::max(new_cost, old_cost); + } + } + } + } + +private: + rclcpp::Node * node_; + + double inflation_radius_; + double robot_radius_; +}; + +#endif // NAVYU_COSTMAP_2D__PLUGINS__INFLATION_LAYER_HPP_ diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp new file mode 100644 index 0000000..d05f67a --- /dev/null +++ b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp @@ -0,0 +1,23 @@ +#ifndef NAVYU_COSTMAP_2D__PLUGINS__LAYER_HPP_ +#define NAVYU_COSTMAP_2D__PLUGINS__LAYER_HPP_ + +#include + +#include + +static constexpr int8_t LETHAL_COST = 100; +static constexpr int8_t INSCRIBED_COST = 99; + +class Layer +{ +public: + explicit Layer(rclcpp::Node * node) { node_ = node; } + virtual ~Layer() {} + virtual void initialize() = 0; + virtual void update(nav_msgs::msg::OccupancyGrid::SharedPtr & master_costmap) = 0; + +private: + rclcpp::Node * node_; +}; + +#endif // NAVYU_COSTMAP_2D__PLUGINS__LAYER_HPP_ diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp new file mode 100644 index 0000000..02f2a54 --- /dev/null +++ b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp @@ -0,0 +1,40 @@ +#ifndef NAVYU_COSTMAP_2D__PLUGINS__STATIC_LAYER_HPP_ +#define NAVYU_COSTMAP_2D__PLUGINS__STATIC_LAYER_HPP_ + +#include "navyu_costmap_2d/plugins/layer.hpp" + +#include + +class StaticLayer : public Layer +{ +public: + explicit StaticLayer(rclcpp::Node * node) : Layer(node) { node_ = node; } + + void initialize() override + { + rclcpp::QoS qos(10); + qos.transient_local(); + qos.reliable(); + qos.keep_last(1); + + const std::string map_topic = node_->declare_parameter("static_layer.map_topic"); + map_subscriber_ = node_->create_subscription( + map_topic, qos, std::bind(&StaticLayer::callback_map, this, std::placeholders::_1)); + } + + void callback_map(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { costmap_ = msg; } + + void update(nav_msgs::msg::OccupancyGrid::SharedPtr & master_costmap) override + { + master_costmap = costmap_; + } + +private: + rclcpp::Node * node_; + + rclcpp::Subscription::SharedPtr map_subscriber_; + + nav_msgs::msg::OccupancyGrid::SharedPtr costmap_; +}; + +#endif // NAVYU_COSTMAP_2D__PLUGINS__STATIC_LAYER_HPP_ diff --git a/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py b/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py new file mode 100644 index 0000000..2a03445 --- /dev/null +++ b/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py @@ -0,0 +1,58 @@ +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + + rviz_config_path = PathJoinSubstitution( + [FindPackageShare("navyu_costmap_2d"), "rviz", "costmap_test.rviz"] + ) + + costmap_map_config_path = PathJoinSubstitution( + [FindPackageShare("navyu_costmap_2d"), "config", "navyu_costmap_2d_params.yaml"] + ) + + map_path = PathJoinSubstitution([FindPackageShare("navyu_navigation"), "map", "map.yaml"]) + + return LaunchDescription( + [ + DeclareLaunchArgument("use_sim_time", default_value="true"), + Node( + package="navyu_costmap_2d", + executable="navyu_costmap_2d_node", + name="global_costmap_node", + output="screen", + parameters=[costmap_map_config_path, {"use_sim_time": use_sim_time}], + ), + Node( + package="nav2_map_server", + executable="map_server", + name="map_server", + parameters=[{"yaml_filename": map_path}, {"frame_id": "map"}], + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="map_server_lifecycle_manager", + output="screen", + emulate_tty=True, + parameters=[ + {"use_sim_time": True}, + {"autostart": True}, + {"node_names": ["map_server"]}, + ], + ), + Node( + package="rviz2", + executable="rviz2", + output="screen", + arguments=["-d", rviz_config_path], + ), + ] + ) diff --git a/navyu_costmap_2d/package.xml b/navyu_costmap_2d/package.xml new file mode 100644 index 0000000..f9ef514 --- /dev/null +++ b/navyu_costmap_2d/package.xml @@ -0,0 +1,28 @@ + + + navyu_costmap_2d + 0.1.0 + The navyu_costmap_2d package + RyuYamamoto + Apache License 2.0 + + ament_cmake_auto + + geometry_msgs + nav_msgs + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + + launch_ros + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/navyu_costmap_2d/src/navyu_costmap_2d.cpp b/navyu_costmap_2d/src/navyu_costmap_2d.cpp new file mode 100644 index 0000000..a9cf767 --- /dev/null +++ b/navyu_costmap_2d/src/navyu_costmap_2d.cpp @@ -0,0 +1,81 @@ +#include "navyu_costmap_2d/navyu_costmap_2d.hpp" + +#include "navyu_costmap_2d/plugins/inflation_layer.hpp" +#include "navyu_costmap_2d/plugins/static_layer.hpp" + +NavyuCostmap2D::NavyuCostmap2D(const rclcpp::NodeOptions & node_options) +: Node("navyu_costmap_2d", node_options) +{ + update_frequency_ = declare_parameter("update_frequency"); + base_frame_id_ = declare_parameter("base_frame_id"); + map_frame_id_ = declare_parameter("map_frame_id"); + + plugins_ = declare_parameter>("plugins"); + + // create all costmap plugins instance + layer_function_["static_layer"] = std::make_shared(this); + layer_function_["inflation_layer"] = std::make_shared(this); + + for (auto plugin : plugins_) layer_function_[plugin]->initialize(); + + rclcpp::QoS qos(10); + qos.transient_local(); + qos.reliable(); + qos.keep_last(1); + + costmap_publisher_ = create_publisher("costmap", qos); + + timer_ = create_wall_timer( + std::chrono::milliseconds(static_cast(1.0 / update_frequency_ * 1000)), + std::bind(&NavyuCostmap2D::update, this)); +} + +NavyuCostmap2D::~NavyuCostmap2D() +{ +} + +void NavyuCostmap2D::update() +{ + nav_msgs::msg::OccupancyGrid::SharedPtr master_costmap; + + // update costmap + for (auto & plugin : plugins_) { + layer_function_[plugin]->update(master_costmap); + } + + if (master_costmap == nullptr) { + RCLCPP_ERROR_STREAM(get_logger(), "master costmap is null"); + return; + } + + master_costmap->header.stamp = now(); + master_costmap->header.frame_id = map_frame_id_; + + costmap_publisher_->publish(*master_costmap); +} + +geometry_msgs::msg::TransformStamped NavyuCostmap2D::get_transform( + const std::string target_frame, const std::string source_frame) +{ + geometry_msgs::msg::TransformStamped frame_transform; + try { + frame_transform = tf_buffer_.lookupTransform( + target_frame, source_frame, tf2::TimePointZero, tf2::durationFromSec(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + frame_transform.header.stamp = rclcpp::Clock().now(); + frame_transform.header.frame_id = target_frame; + frame_transform.child_frame_id = source_frame; + frame_transform.transform.translation.x = 0.0; + frame_transform.transform.translation.y = 0.0; + frame_transform.transform.translation.z = 0.0; + frame_transform.transform.rotation.w = 1.0; + frame_transform.transform.rotation.x = 0.0; + frame_transform.transform.rotation.y = 0.0; + frame_transform.transform.rotation.z = 0.0; + } + return frame_transform; +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(NavyuCostmap2D)