// // /*********************************************************************
// //  *
// //  * 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
// //  *********************************************************************/
// // #ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
// // #define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_

// // #include <vector>
// // #include <list>
// // #include <string>
// // #include <robot/robot.h>
// // #include <robot_costmap_2d/observation.h>
// // #include <tf3/buffer_core.h>
// // #include <robot_sensor_msgs/PointCloud2.h>

// // // Thread support
// // #include <boost/thread.hpp>

// // namespace robot_costmap_2d
// // {
// // /**
// //  * @class ObservationBuffer
// //  * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them
// //  */
// // class ObservationBuffer
// // {
// // public:
// //   /**
// //    * @brief  Constructs an observation buffer
// //    * @param  topic_name The topic of the observations, used as an identifier for error and warning messages
// //    * @param  observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
// //    * @param  expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
// //    * @param  min_obstacle_height The minimum height of a hitpoint to be considered legal
// //    * @param  max_obstacle_height The minimum height of a hitpoint to be considered legal
// //    * @param  obstacle_range The range to which the sensor should be trusted for inserting obstacles
// //    * @param  raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
// //    * @param  tf2_buffer A reference to a tf2 Buffer
// //    * @param  global_frame The frame to transform PointClouds into
// //    * @param  sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
// //    * @param  tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
// //    */
// //   ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
// //                     double min_obstacle_height, double max_obstacle_height, double obstacle_range,
// //                     double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
// //                     std::string sensor_frame, double tf_tolerance);

// //   /**
// //    * @brief  Destructor... cleans up
// //    */
// //   ~ObservationBuffer();

// //   /**
// //    * @brief Sets the global frame of an observation buffer. This will
// //    * transform all the currently cached observations to the new global
// //    * frame
// //    * @param new_global_frame The name of the new global frame.
// //    * @return True if the operation succeeds, false otherwise
// //    */
// //   bool setGlobalFrame(const std::string new_global_frame);

// //   /**
// //    * @brief  Transforms a PointCloud to the global frame and buffers it
// //    * <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
// //    * @param  cloud The cloud to be buffered
// //    */
// //   void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);

// //   /**
// //    * @brief  Pushes copies of all current observations onto the end of the vector passed in
// //    * @param  observations The vector to be filled
// //    */
// //   void getObservations(std::vector<Observation>& observations);

// //   /**
// //    * @brief  Check if the observation buffer is being update at its expected rate
// //    * @return True if it is being updated at the expected rate, false otherwise
// //    */
// //   bool isCurrent() const;

// //   /**
// //    * @brief  Lock the observation buffer
// //    */
// //   inline void lock()
// //   {
// //     lock_.lock();
// //   }

// //   /**
// //    * @brief  Lock the observation buffer
// //    */
// //   inline void unlock()
// //   {
// //     lock_.unlock();
// //   }

// //   /**
// //    * @brief Reset last updated timestamp
// //    */
// //   void resetLastUpdated();

// // private:
// //   /**
// //    * @brief  Removes any stale observations from the buffer list
// //    */
// //   void purgeStaleObservations();

// //   // Helper: trích 4×4 transform matrix từ TransformStampedMsg
// //   // Tránh gọi tf3::doTransform per-point (overhead virtual dispatch + exception check)
// //   struct Transform4x4 {
// //       double m[4][4];
// //   };

// //   static inline Transform4x4 extractMatrix(const tf3::TransformStampedMsg& tfm)
// //   {
// //       // Quaternion → rotation matrix + translation
// //       const auto& t = tfm.transform.translation;
// //       const auto& q = tfm.transform.rotation;

// //       double qx = q.x, qy = q.y, qz = q.z, qw = q.w;
// //       Transform4x4 M;
// //       M.m[0][0] = 1 - 2*(qy*qy + qz*qz); M.m[0][1] = 2*(qx*qy - qz*qw); M.m[0][2] = 2*(qx*qz + qy*qw); M.m[0][3] = t.x;
// //       M.m[1][0] = 2*(qx*qy + qz*qw); M.m[1][1] = 1 - 2*(qx*qx + qz*qz); M.m[1][2] = 2*(qy*qz - qx*qw); M.m[1][3] = t.y;
// //       M.m[2][0] = 2*(qx*qz - qy*qw); M.m[2][1] = 2*(qy*qz + qx*qw); M.m[2][2] = 1 - 2*(qx*qx + qy*qy); M.m[2][3] = t.z;
// //       M.m[3][0] = 0; M.m[3][1] = 0; M.m[3][2] = 0; M.m[3][3] = 1;
// //       return M;
// //   }

// //   double voxel_size_;
// //   tf3::BufferCore& tf3_buffer_;
// //   const robot::Duration observation_keep_time_;
// //   const robot::Duration expected_update_rate_;
// //   robot::Time last_updated_;
// //   std::string global_frame_;
// //   std::string sensor_frame_;
// //   std::list<Observation> observation_list_;
// //   std::string topic_name_;
// //   double min_obstacle_height_, max_obstacle_height_;
// //   boost::recursive_mutex lock_;  ///< @brief A lock for accessing data in callbacks safely
// //   double obstacle_range_, raytrace_range_;
// //   double tf_tolerance_;
// // };
// // }  // namespace robot_costmap_2d
// // #endif  // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_

// /*********************************************************************
//  *
//  * 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
//  *********************************************************************/
// #ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
// #define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_

// #include <vector>
// #include <list>
// #include <string>
// #include <unordered_map>
// #include <cmath>
// #include <cstring>

// #include <robot/robot.h>
// #include <robot_costmap_2d/observation.h>
// #include <tf3/buffer_core.h>
// #include <robot_sensor_msgs/PointCloud2.h>

// // Thread support
// #include <boost/thread.hpp>

// namespace robot_costmap_2d
// {
// /**
//  * @class ObservationBuffer
//  * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them.
//  *
//  * Optimizations vs original:
//  *  - bufferCloud: single-pass transform+filter+voxel-downsample.
//  *    Reduces 6.5 M points to at most (map_w × map_h) representative points,
//  *    which cuts CPU in updateBounds/raytraceFreespace by ~100–200×.
//  *  - extractMatrix: inline quaternion→rotation, avoids per-point virtual dispatch.
//  *  - voxel_size_ (default = costmap resolution, 0.05 m): configurable via
//  *    setVoxelSize() so ObstacleLayer can pass the real resolution.
//  */
// class ObservationBuffer
// {
// public:
//   /**
//    * @brief  Constructs an observation buffer
//    * @param  topic_name The topic of the observations, used as an identifier for error and warning messages
//    * @param  observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
//    * @param  expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
//    * @param  min_obstacle_height The minimum height of a hitpoint to be considered legal
//    * @param  max_obstacle_height The maximum height of a hitpoint to be considered legal
//    * @param  obstacle_range The range to which the sensor should be trusted for inserting obstacles
//    * @param  raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
//    * @param  tf2_buffer A reference to a tf2 Buffer
//    * @param  global_frame The frame to transform PointClouds into
//    * @param  sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
//    * @param  tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
//    */
//   ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
//                     double min_obstacle_height, double max_obstacle_height, double obstacle_range,
//                     double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
//                     std::string sensor_frame, double tf_tolerance);

//   /**
//    * @brief  Destructor... cleans up
//    */
//   ~ObservationBuffer();

//   /**
//    * @brief Sets the global frame of an observation buffer. This will
//    * transform all the currently cached observations to the new global frame
//    * @param new_global_frame The name of the new global frame.
//    * @return True if the operation succeeds, false otherwise
//    */
//   bool setGlobalFrame(const std::string new_global_frame);

//   /**
//    * @brief  Set the voxel size used for downsampling in bufferCloud().
//    *         Should match the costmap resolution (default 0.05 m).
//    */
//   inline void setVoxelSize(double voxel_size)
//   {
//     voxel_size_     = voxel_size;
//     inv_voxel_size_ = (voxel_size > 1e-9) ? 1.0 / voxel_size : 20.0;
//   }

//   /**
//    * @brief  Transforms a PointCloud to the global frame, downsamples it via
//    *         voxel grid (one representative point per costmap cell), applies
//    *         height filtering, and buffers the result.
//    */
//   void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);

//   /**
//    * @brief  Pushes copies of all current observations onto the end of the vector passed in
//    * @param  observations The vector to be filled
//    */
//   void getObservations(std::vector<Observation>& observations);

//   /**
//    * @brief  Check if the observation buffer is being updated at its expected rate
//    * @return True if it is being updated at the expected rate, false otherwise
//    */
//   bool isCurrent() const;

//   /**
//    * @brief  Lock the observation buffer
//    */
//   inline void lock()   { lock_.lock(); }

//   /**
//    * @brief  Unlock the observation buffer
//    */
//   inline void unlock() { lock_.unlock(); }

//   /**
//    * @brief Reset last updated timestamp
//    */
//   void resetLastUpdated();

// private:
//   /**
//    * @brief  Removes any stale observations from the buffer list
//    */
//   void purgeStaleObservations();

//   // ── Transform helper ────────────────────────────────────────────────────
//   // Encode a TF transform as a plain 4×4 double matrix so the hot loop in
//   // bufferCloud can do a simple FMA multiply without any virtual dispatch,
//   // exception handling, or iterator overhead.
//   struct Transform4x4
//   {
//     double m[4][4];
//   };

//   static inline Transform4x4 extractMatrix(const tf3::TransformStampedMsg& tfm)
//   {
//     const auto& t = tfm.transform.translation;
//     const auto& q = tfm.transform.rotation;

//     const double qx = q.x, qy = q.y, qz = q.z, qw = q.w;
//     Transform4x4 M;
//     // Row 0
//     M.m[0][0] = 1.0 - 2.0*(qy*qy + qz*qz);
//     M.m[0][1] = 2.0*(qx*qy - qz*qw);
//     M.m[0][2] = 2.0*(qx*qz + qy*qw);
//     M.m[0][3] = t.x;
//     // Row 1
//     M.m[1][0] = 2.0*(qx*qy + qz*qw);
//     M.m[1][1] = 1.0 - 2.0*(qx*qx + qz*qz);
//     M.m[1][2] = 2.0*(qy*qz - qx*qw);
//     M.m[1][3] = t.y;
//     // Row 2
//     M.m[2][0] = 2.0*(qx*qz - qy*qw);
//     M.m[2][1] = 2.0*(qy*qz + qx*qw);
//     M.m[2][2] = 1.0 - 2.0*(qx*qx + qy*qy);
//     M.m[2][3] = t.z;
//     // Row 3 (homogeneous)
//     M.m[3][0] = 0.0; M.m[3][1] = 0.0; M.m[3][2] = 0.0; M.m[3][3] = 1.0;
//     return M;
//   }

//   // ── Data members ────────────────────────────────────────────────────────
//   tf3::BufferCore&           tf3_buffer_;
//   const robot::Duration      observation_keep_time_;
//   const robot::Duration      expected_update_rate_;
//   robot::Time                last_updated_;
//   std::string                global_frame_;
//   std::string                sensor_frame_;
//   std::list<Observation>     observation_list_;
//   std::string                topic_name_;
//   double                     min_obstacle_height_;
//   double                     max_obstacle_height_;
//   boost::recursive_mutex     lock_;
//   double                     obstacle_range_;
//   double                     raytrace_range_;
//   double                     tf_tolerance_;

//   // Voxel-grid downsampling parameters (set via setVoxelSize)
//   double                     voxel_size_     = 0.05;   // metres – match costmap resolution
//   double                     inv_voxel_size_ = 20.0;   // 1/voxel_size_, cached
// };

// }  // namespace robot_costmap_2d
// #endif  // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
/*********************************************************************
 *
 * 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
 *********************************************************************/
#ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
#define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_

#include <vector>
#include <list>
#include <string>
#include <robot/robot.h>
#include <robot_costmap_2d/observation.h>
#include <tf3/buffer_core.h>
#include <robot_sensor_msgs/PointCloud2.h>

// Thread support
#include <boost/thread.hpp>

namespace robot_costmap_2d
{
/**
 * @class ObservationBuffer
 * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them
 */
class ObservationBuffer
{
public:
  /**
   * @brief  Constructs an observation buffer
   * @param  topic_name The topic of the observations, used as an identifier for error and warning messages
   * @param  observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
   * @param  expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
   * @param  min_obstacle_height The minimum height of a hitpoint to be considered legal
   * @param  max_obstacle_height The minimum height of a hitpoint to be considered legal
   * @param  obstacle_range The range to which the sensor should be trusted for inserting obstacles
   * @param  raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
   * @param  tf2_buffer A reference to a tf2 Buffer
   * @param  global_frame The frame to transform PointClouds into
   * @param  sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
   * @param  tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
   */
  ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
                    double min_obstacle_height, double max_obstacle_height, double obstacle_range,
                    double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
                    std::string sensor_frame, double tf_tolerance);

  /**
   * @brief  Destructor... cleans up
   */
  ~ObservationBuffer();

  /**
   * @brief Sets the global frame of an observation buffer. This will
   * transform all the currently cached observations to the new global
   * frame
   * @param new_global_frame The name of the new global frame.
   * @return True if the operation succeeds, false otherwise
   */
  bool setGlobalFrame(const std::string new_global_frame);

  /**
   * @brief  Transforms a PointCloud to the global frame and buffers it
   * <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
   * @param  cloud The cloud to be buffered
   */
  void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);

  /**
   * @brief  Pushes copies of all current observations onto the end of the vector passed in
   * @param  observations The vector to be filled
   */
  void getObservations(std::vector<Observation>& observations);

  /**
   * @brief  Check if the observation buffer is being update at its expected rate
   * @return True if it is being updated at the expected rate, false otherwise
   */
  bool isCurrent() const;

  /**
   * @brief  Lock the observation buffer
   */
  inline void lock()
  {
    lock_.lock();
  }

  /**
   * @brief  Lock the observation buffer
   */
  inline void unlock()
  {
    lock_.unlock();
  }

  /**
   * @brief Reset last updated timestamp
   */
  void resetLastUpdated();

private:
  /**
   * @brief  Removes any stale observations from the buffer list
   */
  void purgeStaleObservations();

  tf3::BufferCore& tf3_buffer_;
  const robot::Duration observation_keep_time_;
  const robot::Duration expected_update_rate_;
  robot::Time last_updated_;
  std::string global_frame_;
  std::string sensor_frame_;
  std::list<Observation> observation_list_;
  std::string topic_name_;
  double min_obstacle_height_, max_obstacle_height_;
  boost::recursive_mutex lock_;  ///< @brief A lock for accessing data in callbacks safely
  double obstacle_range_, raytrace_range_;
  double tf_tolerance_;
};
}  // namespace robot_costmap_2d
#endif  // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_