DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add function computeCost file layer.h

Browse Source
This commit is contained in:
duongtd
2026-07-10 11:27:20 +07:00
parent 2fcd211ccf
commit a2a021c114
10 changed files with 1355 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

388
include/robot_costmap_2d/observation_buffer.h
View File

@@ -1,3 +1,391 @@
// // /*********************************************************************
// // *
// // * 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)