clear_costmap_service.hpp

// 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.
 
#ifndef NAV2_COSTMAP_2D__CLEAR_COSTMAP_SERVICE_HPP_
#define NAV2_COSTMAP_2D__CLEAR_COSTMAP_SERVICE_HPP_
 
#include <vector>
#include <string>
#include <memory>
#include <functional>
 
#include "rclcpp/rclcpp.hpp"
#include "nav2_msgs/srv/clear_costmap_except_region.hpp"
#include "nav2_msgs/srv/clear_costmap_around_robot.hpp"
#include "nav2_msgs/srv/clear_costmap_around_pose.hpp"
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
#include "nav2_costmap_2d/costmap_layer.hpp"
#include "nav2_ros_common/lifecycle_node.hpp"
#include "nav2_ros_common/service_server.hpp"
 
namespace nav2_costmap_2d
{
 
class Costmap2DROS;
 
class ClearCostmapService
{
public:
  ClearCostmapService(
    const nav2::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap);
 
  ClearCostmapService() = delete;
 
  ~ClearCostmapService();
 
  bool clearRegion(double reset_distance, bool invert, const std::vector<std::string> & plugins);
 
  bool clearAroundPose(
    const geometry_msgs::msg::PoseStamped & pose, double reset_distance,
    const std::vector<std::string> & plugins);
 
  bool clearEntirely(const std::vector<std::string> & plugins);
 
private:
  // The Logger object for logging
  rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")};
 
  // The costmap to clear
  Costmap2DROS & costmap_;
 
  // Clearing parameters
  unsigned char reset_value_;
 
  // Server for clearing the costmap
  nav2::ServiceServer<nav2_msgs::srv::ClearCostmapExceptRegion>::SharedPtr
    clear_except_service_;
  void clearExceptRegionCallback(
    const std::shared_ptr<rmw_request_id_t> request_header,
    const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Request> request,
    const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Response> response);
 
  nav2::ServiceServer<nav2_msgs::srv::ClearCostmapAroundRobot>::SharedPtr
    clear_around_service_;
  void clearAroundRobotCallback(
    const std::shared_ptr<rmw_request_id_t> request_header,
    const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Request> request,
    const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Response> response);
 
  nav2::ServiceServer<nav2_msgs::srv::ClearCostmapAroundPose>::SharedPtr
    clear_around_pose_service_;
  void clearAroundPoseCallback(
    const std::shared_ptr<rmw_request_id_t> request_header,
    const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundPose::Request> request,
    const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundPose::Response> response);
 
  nav2::ServiceServer<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr
    clear_entire_service_;
  void clearEntireCallback(
    const std::shared_ptr<rmw_request_id_t> request_header,
    const std::shared_ptr<nav2_msgs::srv::ClearEntireCostmap::Request> request,
    const std::shared_ptr<nav2_msgs::srv::ClearEntireCostmap::Response> response);
 
  void clearLayerRegion(
    std::shared_ptr<CostmapLayer> & costmap, double pose_x, double pose_y, double reset_distance,
    bool invert);
 
  bool getPosition(double & x, double & y) const;
 
  void validatePlugins(
    const std::vector<std::string> & requested_plugins,
    const std::vector<std::shared_ptr<Layer>> * layers,
    std::vector<std::string> & invalid_plugins) const;
 
  bool validateAndClearPlugins(
    const std::vector<std::string> & plugins,
    const std::vector<std::shared_ptr<Layer>> * layers,
    std::function<void(std::shared_ptr<CostmapLayer> &)> clear_callback,
    const std::string & operation_name) const;
};
 
}  // namespace nav2_costmap_2d
 
#endif  // NAV2_COSTMAP_2D__CLEAR_COSTMAP_SERVICE_HPP_