Hiep update

include/robot_costmap_2d/layer.h

@@ -0,0 +1,169 @@
+/*********************************************************************
+ *
+ * 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: David V. Lu!!
+ *********************************************************************/
+#ifndef ROBOT_COSTMAP_2D_LAYER_H_
+#define ROBOT_COSTMAP_2D_LAYER_H_
+
+#include <robot_costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/utils.h>
+#include <string>
+#include <tf3/buffer_core.h>
+#include <robot/node_handle.h>
+namespace robot_costmap_2d
+{
+class LayeredCostmap;
+
+class Layer
+{
+public:
+  Layer();
+
+  void initialize(LayeredCostmap* parent, std::string name, tf3::BufferCore *tf);
+
+  /**
+   * @brief This is called by the LayeredCostmap to poll this plugin as to how
+   *        much of the costmap it needs to update. Each layer can increase
+   *        the size of this bounds.
+   *
+   * For more details, see "Layered Costmaps for Context-Sensitive Navigation",
+   * by Lu et. Al, IROS 2014.
+   */
+  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
+                            double* max_x, double* max_y) {}
+
+  /**
+   * @brief Actually update the underlying costmap, only within the bounds
+   *        calculated during UpdateBounds().
+   */
+  virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {}
+
+  /** @brief Stop publishers. */
+  virtual void deactivate() {}
+
+  /** @brief Restart publishers if they've been stopped. */
+  virtual void activate() {}
+
+  virtual void reset() {}
+
+  virtual ~Layer() {}
+
+  /**
+   * @brief Check to make sure all the data in the layer is up to date.
+   *        If the layer is not up to date, then it may be unsafe to
+   *        plan using the data from this layer, and the planner may
+   *        need to know.
+   *
+   *        A layer's current state should be managed by the protected
+   *        variable current_.
+   * @return Whether the data in the layer is up to date.
+   */
+  bool isCurrent() const
+  {
+    return current_;
+  }
+
+  /**
+   * @brief getter if the current layer is enabled.
+   *
+   * The user may enable/disable a layer though dynamic reconfigure.
+   * Disabled layers won't receive calls to
+   * - Layer::updateCosts
+   * - Layer::updateBounds
+   * - Layer::isCurrent
+   * from the LayeredCostmap.
+   *
+   * Calls to Layer::activate, Layer::deactivate and Layer::reset won't be
+   * blocked.
+   */
+  inline bool isEnabled() const noexcept
+  {
+    return enabled_;
+  }
+
+  /** @brief Implement this to make this layer match the size of the parent costmap. */
+  virtual void matchSize() {}
+
+  inline const std::string& getName() const noexcept
+  {
+    return name_;
+  }
+
+  /** @brief Convenience function for layered_costmap_->getFootprint(). */
+  const std::vector<robot_geometry_msgs::Point>& getFootprint() const;
+
+  /** @brief LayeredCostmap calls this whenever the footprint there
+   * changes (via LayeredCostmap::setFootprint()).  Override to be
+   * notified of changes to the robot's footprint. */
+  virtual void onFootprintChanged() {}
+
+  template<typename T>
+  void dataCallBack(const T& value, const std::string& topic) {
+      handle(value, topic);
+  }
+protected:
+
+  // Hàm template public, dùng để gửi dữ liệu
+  template<typename T>
+  void handle(const T& data, const std::string& topic) {
+      handleImpl(static_cast<const void*>(&data), typeid(T), topic);
+  }
+
+  // Hàm ảo duy nhất mà plugin sẽ override
+  virtual void handleImpl(const void* data, 
+                          const std::type_info& type, 
+                          const std::string& topic) = 0;
+  /** @brief This is called at the end of initialize().  Override to
+   * implement subclass-specific initialization.
+   *
+   * tf_, name_, and layered_costmap_ will all be set already when this is called. */
+  virtual void onInitialize() {}
+
+  LayeredCostmap* layered_costmap_;
+  bool current_;
+  bool enabled_;
+  std::string name_;
+  tf3::BufferCore *tf_;
+
+private:
+  std::vector<robot_geometry_msgs::Point> footprint_spec_;
+};
+
+using PluginLayerPtr = boost::shared_ptr<Layer>;
+
+}  // namespace robot_costmap_2d
+
+#endif  // ROBOT_COSTMAP_2D_LAYER_H_