costmap_2d_ros.hpp

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  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!!
 *********************************************************************/
#ifndef NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_
#define NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_
 
#include <atomic>
#include <memory>
#include <string>
#include <vector>
 
#include "geometry_msgs/msg/polygon.hpp"
#include "geometry_msgs/msg/polygon_stamped.hpp"
#include "nav2_costmap_2d/costmap_2d_publisher.hpp"
#include "nav2_costmap_2d/footprint.hpp"
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
#include "nav2_costmap_2d/clear_costmap_service.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/layer.hpp"
#include "nav2_ros_common/lifecycle_node.hpp"
#include "nav2_msgs/srv/get_costs.hpp"
#include "pluginlib/class_loader.hpp"
#include "tf2/convert.hpp"
#include "tf2/LinearMath/Transform.hpp"
#include "nav2_ros_common/tf2_factories.hpp"
#include "tf2/time.hpp"
#include "tf2/transform_datatypes.hpp"
#include "nav2_ros_common/service_server.hpp"
 
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include "tf2/utils.hpp"
#pragma GCC diagnostic pop
 
namespace nav2_costmap_2d
{
 
class Costmap2DROS : public nav2::LifecycleNode
{
public:
  explicit Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
  explicit Costmap2DROS(
    const std::string & name,
    const std::string & parent_namespace = "/",
    const bool & use_sim_time = false,
    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
  void init();
 
  ~Costmap2DROS();
 
  nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
 
  nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
 
  nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
 
  nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
 
  nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
  void on_rcl_preshutdown() override
  {
    if (is_lifecycle_follower_) {
      // Transitioning handled by parent node
      return;
    }
 
    // Else, if this is an independent node, this node needs to handle itself.
    RCLCPP_INFO(
      get_logger(), "Running Nav2 LifecycleNode rcl preshutdown (%s)",
      this->get_name());
 
    runCleanups();
 
    destroyBond();
  }
 
  void start();
 
  void stop();
 
  void pause();
 
  void resume();
 
  void updateMap();
 
  void resetLayers();
 
  bool isCurrent()
  {
    return layered_costmap_->isCurrent();
  }
 
  void waitUntilCurrent(const rclcpp::Duration & timeout);
 
  bool getRobotPose(geometry_msgs::msg::PoseStamped & global_pose);
 
  bool transformPoseToGlobalFrame(
    const geometry_msgs::msg::PoseStamped & input_pose,
    geometry_msgs::msg::PoseStamped & transformed_pose);
 
  std::string getName() const
  {
    return name_;
  }
 
  double getTransformTolerance() const
  {
    return transform_tolerance_;
  }
 
  Costmap2D * getCostmap()
  {
    return layered_costmap_->getCostmap();
  }
 
  std::string getGlobalFrameID()
  {
    return global_frame_;
  }
 
  std::string getBaseFrameID()
  {
    return robot_base_frame_;
  }
 
  LayeredCostmap * getLayeredCostmap()
  {
    return layered_costmap_.get();
  }
 
  geometry_msgs::msg::Polygon getRobotFootprintPolygon()
  {
#ifdef __cpp_lib_atomic_shared_ptr
    return nav2_costmap_2d::toPolygon(*padded_footprint_.load());
#else
    return nav2_costmap_2d::toPolygon(*std::atomic_load(&padded_footprint_));
#endif
  }
 
  std::vector<geometry_msgs::msg::Point> getRobotFootprint()
  {
#ifdef __cpp_lib_atomic_shared_ptr
    return *padded_footprint_.load();
#else
    return *std::atomic_load(&padded_footprint_);
#endif
  }
 
  std::vector<geometry_msgs::msg::Point> getUnpaddedRobotFootprint()
  {
#ifdef __cpp_lib_atomic_shared_ptr
    return *unpadded_footprint_.load();
#else
    return *std::atomic_load(&unpadded_footprint_);
#endif
  }
 
  void getOrientedFootprint(std::vector<geometry_msgs::msg::Point> & oriented_footprint);
 
  void setRobotFootprint(const std::vector<geometry_msgs::msg::Point> & points);
 
  void setRobotFootprintPolygon(const geometry_msgs::msg::Polygon & footprint);
 
  nav2::TransformBuffer::SharedPtr getTfBuffer() {return tf_buffer_;}
 
  bool getUseRadius() {return use_radius_;}
 
  double getRobotRadius() {return robot_radius_;}
 
  void getCostsCallback(
    const std::shared_ptr<rmw_request_id_t>,
    const std::shared_ptr<nav2_msgs::srv::GetCosts::Request> request,
    const std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response);
 
protected:
  // Publishers and subscribers
  nav2::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
    footprint_pub_;
  std::unique_ptr<Costmap2DPublisher> costmap_publisher_;
 
  std::vector<std::unique_ptr<Costmap2DPublisher>> layer_publishers_;
 
  nav2::Subscription<geometry_msgs::msg::Polygon>::SharedPtr footprint_sub_;
  nav2::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_stamped_sub_;
  nav2::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_sub_;
 
  // Dedicated callback group and executor for tf timer_interface and message filter
  rclcpp::CallbackGroup::SharedPtr callback_group_;
  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
  std::unique_ptr<nav2::NodeThread> executor_thread_;
 
  // Transform listener
  nav2::TransformBuffer::SharedPtr tf_buffer_;
  nav2::TransformListener::SharedPtr tf_listener_;
 
  std::unique_ptr<LayeredCostmap> layered_costmap_{nullptr};
  std::string name_;
 
  void mapUpdateLoop(double frequency);
  std::atomic<bool> map_update_thread_shutdown_{false};  // [AI generated]
  std::atomic<bool> stop_updates_{false};
  std::atomic<bool> initialized_{false};
  std::atomic<bool> stopped_{true};
  std::mutex _dynamic_parameter_mutex;
  std::unique_ptr<std::thread> map_update_thread_;  
  rclcpp::Time last_publish_{0, 0, RCL_ROS_TIME};
  rclcpp::Duration publish_cycle_{1, 0};
  pluginlib::ClassLoader<Layer> plugin_loader_{"nav2_costmap_2d", "nav2_costmap_2d::Layer"};
 
  void getParameters();
  bool always_send_full_costmap_{false};
  std::string footprint_;
  float footprint_padding_{0};
  std::string global_frame_;                
  int map_height_meters_{0};
  double map_publish_frequency_{0};
  double map_update_frequency_{0};
  int map_width_meters_{0};
  double origin_x_{0};
  double origin_y_{0};
  std::vector<std::string> default_plugins_;
  std::vector<std::string> default_types_;
  std::vector<std::string> plugin_names_;
  std::vector<std::string> plugin_types_;
  std::vector<std::string> filter_names_;
  std::vector<std::string> filter_types_;
  double resolution_{0};
  std::string robot_base_frame_;            
  double robot_radius_;
  bool rolling_window_{false};          
  bool track_unknown_space_{false};
  double transform_tolerance_{0};           
  double initial_transform_timeout_{0};   
  double map_vis_z_{0};                 
  bool subscribe_to_stamped_footprint_{false};
 
  bool is_lifecycle_follower_{true};   
 
  // Derived parameters
  bool use_radius_{false};
#ifdef __cpp_lib_atomic_shared_ptr
  std::atomic<std::shared_ptr<std::vector<geometry_msgs::msg::Point>>> unpadded_footprint_{
    std::make_shared<std::vector<geometry_msgs::msg::Point>>()};
  std::atomic<std::shared_ptr<std::vector<geometry_msgs::msg::Point>>> padded_footprint_{
    std::make_shared<std::vector<geometry_msgs::msg::Point>>()};
#else
  std::shared_ptr<std::vector<geometry_msgs::msg::Point>> unpadded_footprint_{
    std::make_shared<std::vector<geometry_msgs::msg::Point>>()};
  std::shared_ptr<std::vector<geometry_msgs::msg::Point>> padded_footprint_{
    std::make_shared<std::vector<geometry_msgs::msg::Point>>()};
#endif
 
  // Services
  nav2::ServiceServer<nav2_msgs::srv::GetCosts>::SharedPtr get_cost_service_;
  std::unique_ptr<ClearCostmapService> clear_costmap_service_;
 
  // Dynamic parameters handler
  PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_;
  OnSetParametersCallbackHandle::SharedPtr on_set_params_handler;
 
  rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(
    const std::vector<rclcpp::Parameter> & parameters);
 
  void updateParametersCallback(const std::vector<rclcpp::Parameter> & parameters);
};
 
// free functions
 
rclcpp::NodeOptions getChildNodeOptions(
  const std::string & name,
  const std::string & parent_namespace,
  const bool & use_sim_time,
  const rclcpp::NodeOptions & parent_options);
 
}  // namespace nav2_costmap_2d
 
#endif  // NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_