layer.h 2.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101
  1. #ifndef COSTMAP_2D_LAYER_H_
  2. #define COSTMAP_2D_LAYER_H_
  3. #include <costmap_2d/costmap_2d.h>
  4. #include <costmap_2d/layered_costmap.h>
  5. #include <string>
  6. #include <tf2_ros/buffer.h>
  7. namespace costmap_2d
  8. {
  9. class LayeredCostmap;
  10. class Layer
  11. {
  12. public:
  13. Layer();
  14. void initialize(LayeredCostmap* parent, std::string name, tf2_ros::Buffer *tf);
  15. /**
  16. * @brief This is called by the LayeredCostmap to poll this plugin as to how
  17. * much of the costmap it needs to update. Each layer can increase
  18. * the size of this bounds.
  19. *
  20. * For more details, see "Layered Costmaps for Context-Sensitive Navigation",
  21. * by Lu et. Al, IROS 2014.
  22. */
  23. virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) {}
  24. /**
  25. * @brief Actually update the underlying costmap, only within the bounds
  26. * calculated during UpdateBounds().
  27. */
  28. virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {}
  29. /** @brief Stop publishers. */
  30. virtual void deactivate() {}
  31. /** @brief Restart publishers if they've been stopped. */
  32. virtual void activate() {}
  33. virtual void reset() {}
  34. virtual ~Layer() {}
  35. /**
  36. * @brief Check to make sure all the data in the layer is up to date.
  37. * If the layer is not up to date, then it may be unsafe to
  38. * plan using the data from this layer, and the planner may
  39. * need to know.
  40. *
  41. * A layer's current state should be managed by the protected
  42. * variable current_.
  43. * @return Whether the data in the layer is up to date.
  44. */
  45. bool isCurrent() const
  46. {
  47. return current_;
  48. }
  49. /** @brief Implement this to make this layer match the size of the parent costmap. */
  50. virtual void matchSize() {}
  51. std::string getName() const
  52. {
  53. return name_;
  54. }
  55. /** @brief Convenience function for layered_costmap_->getFootprint(). */
  56. const std::vector<geometry_msgs::Point>& getFootprint() const;
  57. /** @brief LayeredCostmap calls this whenever the footprint there
  58. * changes (via LayeredCostmap::setFootprint()). Override to be
  59. * notified of changes to the robot's footprint. */
  60. virtual void onFootprintChanged() {}
  61. protected:
  62. /** @brief This is called at the end of initialize(). Override to
  63. * implement subclass-specific initialization.
  64. *
  65. * tf_, name_, and layered_costmap_ will all be set already when this is called. */
  66. virtual void onInitialize() {}
  67. LayeredCostmap* layered_costmap_;
  68. bool current_;
  69. bool enabled_; ///< Currently this var is managed by subclasses. TODO: make this managed by this class and/or container class.
  70. std::string name_;
  71. tf2_ros::Buffer *tf_;
  72. private:
  73. std::vector<geometry_msgs::Point> footprint_spec_;
  74. };
  75. } // namespace costmap_2d
  76. #endif // COSTMAP_2D_LAYER_H_