add function computeCost file layer.h
This commit is contained in:
@@ -8,4 +8,5 @@ voxel_layer:
|
|||||||
unknown_threshold: 15.0
|
unknown_threshold: 15.0
|
||||||
mark_threshold: 0
|
mark_threshold: 0
|
||||||
combination_method: 1
|
combination_method: 1
|
||||||
|
frustum_clearing_enabled: true
|
||||||
|
frustum_clearing_pixel_step: 8
|
||||||
|
|||||||
@@ -96,8 +96,9 @@ public:
|
|||||||
/** @brief Given a distance, compute a cost.
|
/** @brief Given a distance, compute a cost.
|
||||||
* @param distance The distance from an obstacle in cells
|
* @param distance The distance from an obstacle in cells
|
||||||
* @return A cost value for the distance */
|
* @return A cost value for the distance */
|
||||||
virtual inline unsigned char computeCost(double distance) const
|
virtual unsigned char computeCost(double distance) const override
|
||||||
{
|
{
|
||||||
|
// robot::log_warning("InflationLayer::computeCost() is deprecated. Please use costLookup() instead.");
|
||||||
unsigned char cost = 0;
|
unsigned char cost = 0;
|
||||||
if (distance == 0)
|
if (distance == 0)
|
||||||
cost = LETHAL_OBSTACLE;
|
cost = LETHAL_OBSTACLE;
|
||||||
|
|||||||
@@ -85,6 +85,8 @@ public:
|
|||||||
*/
|
*/
|
||||||
virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {}
|
virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {}
|
||||||
|
|
||||||
|
virtual unsigned char computeCost(double distance) const { throw std::runtime_error("Function computeCost is not Support."); };
|
||||||
|
|
||||||
/** @brief Stop publishers. */
|
/** @brief Stop publishers. */
|
||||||
virtual void deactivate() {}
|
virtual void deactivate() {}
|
||||||
|
|
||||||
|
|||||||
@@ -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)
|
* Software License Agreement (BSD License)
|
||||||
|
|||||||
@@ -91,12 +91,21 @@ private:
|
|||||||
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
|
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
|
||||||
virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
|
virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
|
||||||
double* max_x, double* max_y);
|
double* max_x, double* max_y);
|
||||||
|
bool raytraceDepthFrustum(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
|
||||||
|
double* max_x, double* max_y);
|
||||||
|
bool getCloudPoint(const robot_sensor_msgs::PointCloud2& cloud, unsigned int u, unsigned int v,
|
||||||
|
double& wx, double& wy, double& wz) const;
|
||||||
|
bool clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz);
|
||||||
|
bool clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
|
||||||
|
double raytrace_range, double* min_x, double* min_y, double* max_x, double* max_y);
|
||||||
|
|
||||||
|
|
||||||
bool publish_voxel_;
|
bool publish_voxel_;
|
||||||
robot_voxel_grid::VoxelGrid robot_voxel_grid_;
|
robot_voxel_grid::VoxelGrid robot_voxel_grid_;
|
||||||
double z_resolution_, origin_z_;
|
double z_resolution_, origin_z_;
|
||||||
unsigned int unknown_threshold_, mark_threshold_, size_z_;
|
unsigned int unknown_threshold_, mark_threshold_, size_z_;
|
||||||
|
bool frustum_clearing_enabled_;
|
||||||
|
unsigned int frustum_clearing_pixel_step_;
|
||||||
robot_sensor_msgs::PointCloud clearing_endpoints_;
|
robot_sensor_msgs::PointCloud clearing_endpoints_;
|
||||||
|
|
||||||
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
|
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
|
||||||
|
|||||||
@@ -162,14 +162,6 @@ namespace robot_costmap_2d
|
|||||||
robot_nav_msgs::OccupancyGrid lanes;
|
robot_nav_msgs::OccupancyGrid lanes;
|
||||||
convertToMap(costmap_, lanes, 0.65, 0.196);
|
convertToMap(costmap_, lanes, 0.65, 0.196);
|
||||||
|
|
||||||
//////////////////////////////////
|
|
||||||
//////////////////////////////////
|
|
||||||
/////////THAY THẾ PUBLISH////////
|
|
||||||
// lane_mask_pub_.publish(lanes);
|
|
||||||
//////////////////////////////////
|
|
||||||
//////////////////////////////////
|
|
||||||
//////////////////////////////////
|
|
||||||
|
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -44,6 +44,7 @@
|
|||||||
|
|
||||||
#include <boost/dll/alias.hpp>
|
#include <boost/dll/alias.hpp>
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
|
#include <cxxabi.h>
|
||||||
|
|
||||||
|
|
||||||
using robot_costmap_2d::NO_INFORMATION;
|
using robot_costmap_2d::NO_INFORMATION;
|
||||||
@@ -71,6 +72,7 @@ void StaticLayer::onInitialize()
|
|||||||
global_frame_ = layered_costmap_->getGlobalFrameID();
|
global_frame_ = layered_costmap_->getGlobalFrameID();
|
||||||
std::string config_file_name = "static_layer_params.yaml";
|
std::string config_file_name = "static_layer_params.yaml";
|
||||||
getParams(config_file_name, priv_nh);
|
getParams(config_file_name, priv_nh);
|
||||||
|
robot::log_warning("Initializing static layer with map topic \"%s\" in frame \"%s\"", map_topic_.c_str(), global_frame_.c_str());
|
||||||
}
|
}
|
||||||
|
|
||||||
bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
|
bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
|
||||||
@@ -191,12 +193,23 @@ void StaticLayer::handleImpl(const void* data,
|
|||||||
const std::type_info& type,
|
const std::type_info& type,
|
||||||
const std::string& topic)
|
const std::string& topic)
|
||||||
{
|
{
|
||||||
if (type == typeid(robot_nav_msgs::OccupancyGrid) && topic == map_topic_) {
|
if (type == typeid(robot_nav_msgs::OccupancyGrid) &&
|
||||||
|
(topic == map_topic_ || topic == "/" + map_topic_)) {
|
||||||
incomingMap(*static_cast<const robot_nav_msgs::OccupancyGrid*>(data));
|
incomingMap(*static_cast<const robot_nav_msgs::OccupancyGrid*>(data));
|
||||||
} else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) && topic == map_topic_ + "_updates") {
|
} else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) &&
|
||||||
|
(topic == map_topic_ + "_updates" || topic == "/" + map_topic_ + "_updates")) {
|
||||||
incomingUpdate(*static_cast<const robot_map_msgs::OccupancyGridUpdate*>(data));
|
incomingUpdate(*static_cast<const robot_map_msgs::OccupancyGridUpdate*>(data));
|
||||||
} else {
|
} else {
|
||||||
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
|
std::string readable = boost::core::demangle(type.name());
|
||||||
|
|
||||||
|
size_t pos = readable.find("<");
|
||||||
|
if (pos != std::string::npos)
|
||||||
|
{
|
||||||
|
readable = readable.substr(0, pos);
|
||||||
|
}
|
||||||
|
robot::log_error("[] con1: %x, con2: %x ", type == typeid(robot_nav_msgs::OccupancyGrid), (topic == map_topic_ || topic == "/" + map_topic_));
|
||||||
|
robot::log_error("[StaticLayer] Received data of unknown type: %s on topic: %s, map_topic_: %s\n", readable.c_str(), topic.c_str(), map_topic_.c_str());
|
||||||
|
// std::cout << "[StaticLayer] Unknown type: " << type.name() << " on topic: " << topic << std::endl;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -204,7 +217,7 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
|
|||||||
{
|
{
|
||||||
if(!map_shutdown_)
|
if(!map_shutdown_)
|
||||||
{
|
{
|
||||||
std::cout << "Received new map!" << std::endl;
|
std::cout << "[StaticLayer] Received new map!" << std::endl;
|
||||||
unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
|
unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
|
||||||
|
|
||||||
robot::log_info("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
|
robot::log_info("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
|
||||||
@@ -381,7 +394,7 @@ void StaticLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_
|
|||||||
tf3::TransformStampedMsg transformMsg;
|
tf3::TransformStampedMsg transformMsg;
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time::now());
|
transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time());
|
||||||
}
|
}
|
||||||
catch (tf3::TransformException ex)
|
catch (tf3::TransformException ex)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -331,8 +331,8 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
|
|||||||
// bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
|
// bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
|
||||||
// if (publish_clearing_points)
|
// if (publish_clearing_points)
|
||||||
// {
|
// {
|
||||||
clearing_endpoints_.points.clear();
|
// clearing_endpoints_.points.clear();
|
||||||
clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
|
// clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
|
||||||
// }
|
// }
|
||||||
|
|
||||||
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
|
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
|
||||||
@@ -412,20 +412,20 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
|
|||||||
|
|
||||||
// if (publish_clearing_points)
|
// if (publish_clearing_points)
|
||||||
// {
|
// {
|
||||||
robot_geometry_msgs::Point32 point;
|
// robot_geometry_msgs::Point32 point;
|
||||||
point.x = wpx;
|
// point.x = wpx;
|
||||||
point.y = wpy;
|
// point.y = wpy;
|
||||||
point.z = wpz;
|
// point.z = wpz;
|
||||||
clearing_endpoints_.points.push_back(point);
|
// clearing_endpoints_.points.push_back(point);
|
||||||
// }
|
// }
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// if (publish_clearing_points)
|
// if (publish_clearing_points)
|
||||||
// {
|
// {
|
||||||
clearing_endpoints_.header.frame_id = global_frame_;
|
// clearing_endpoints_.header.frame_id = global_frame_;
|
||||||
clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
|
// clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
|
||||||
clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
|
// clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
|
||||||
|
|
||||||
// clearing_endpoints_pub_.publish(clearing_endpoints_);
|
// clearing_endpoints_pub_.publish(clearing_endpoints_);
|
||||||
// }
|
// }
|
||||||
|
|||||||
@@ -378,15 +378,25 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
|
|||||||
bool clearing;
|
bool clearing;
|
||||||
bool marking;
|
bool marking;
|
||||||
bool inf_is_valid;
|
bool inf_is_valid;
|
||||||
|
std::string sensor_frame;
|
||||||
|
double observation_persistence;
|
||||||
|
double expected_update_rate;
|
||||||
double min_obstacle_height;
|
double min_obstacle_height;
|
||||||
double max_obstacle_height;
|
double max_obstacle_height;
|
||||||
|
double obstacle_range;
|
||||||
|
double raytrace_range;
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "expected_update_rate", expected_update_rate);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "obstacle_range", obstacle_range);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "raytrace_range", raytrace_range);
|
||||||
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
|
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -417,15 +427,25 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
|
|||||||
bool clearing;
|
bool clearing;
|
||||||
bool marking;
|
bool marking;
|
||||||
bool inf_is_valid;
|
bool inf_is_valid;
|
||||||
|
std::string sensor_frame;
|
||||||
|
double observation_persistence;
|
||||||
|
double expected_update_rate;
|
||||||
double min_obstacle_height;
|
double min_obstacle_height;
|
||||||
double max_obstacle_height;
|
double max_obstacle_height;
|
||||||
|
double obstacle_range;
|
||||||
|
double raytrace_range;
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "expected_update_rate", expected_update_rate);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
|
||||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "obstacle_range", obstacle_range);
|
||||||
|
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "raytrace_range", raytrace_range);
|
||||||
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
|
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,3 +1,906 @@
|
|||||||
|
// // /*********************************************************************
|
||||||
|
// // *
|
||||||
|
// // * 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
|
||||||
|
// // *********************************************************************/
|
||||||
|
// // #include <robot_costmap_2d/observation_buffer.h>
|
||||||
|
|
||||||
|
// // #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||||
|
// // #include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
|
||||||
|
// // #include <robot_sensor_msgs/point_cloud2_iterator.h>
|
||||||
|
|
||||||
|
// // using namespace std;
|
||||||
|
// // using namespace tf3;
|
||||||
|
|
||||||
|
// // namespace robot_costmap_2d
|
||||||
|
// // {
|
||||||
|
// // ObservationBuffer::ObservationBuffer(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, string global_frame,
|
||||||
|
// // string sensor_frame, double tf_tolerance) :
|
||||||
|
// // tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
|
||||||
|
// // last_updated_(robot::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
|
||||||
|
// // min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
|
||||||
|
// // obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance), voxel_size_(0.05)
|
||||||
|
// // {
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // ObservationBuffer::~ObservationBuffer()
|
||||||
|
// // {
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
|
||||||
|
// // {
|
||||||
|
// // tf3::Time transform_time = tf3::Time::now();
|
||||||
|
// // std::string tf_error;
|
||||||
|
|
||||||
|
// // robot_geometry_msgs::TransformStamped transformStamped;
|
||||||
|
// // if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
|
||||||
|
// // {
|
||||||
|
// // robot::log_error("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
|
||||||
|
// // global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
|
||||||
|
// // return false;
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // list<Observation>::iterator obs_it;
|
||||||
|
// // for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
|
||||||
|
// // {
|
||||||
|
// // try
|
||||||
|
// // {
|
||||||
|
// // Observation& obs = *obs_it;
|
||||||
|
|
||||||
|
// // robot_geometry_msgs::PointStamped origin;
|
||||||
|
// // origin.header.frame_id = global_frame_;
|
||||||
|
// // origin.header.stamp = data_convert::convertTime(transform_time);
|
||||||
|
// // origin.point = obs.origin_;
|
||||||
|
|
||||||
|
// // // we need to transform the origin of the observation to the new global frame
|
||||||
|
// // // tf3_buffer_.transform(origin, origin, new_global_frame);
|
||||||
|
// // tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
|
||||||
|
// // new_global_frame, // frame đích
|
||||||
|
// // origin.header.frame_id, // frame nguồn
|
||||||
|
// // transform_time
|
||||||
|
// // );
|
||||||
|
// // tf3::doTransform(origin, origin, tfm_1);
|
||||||
|
// // obs.origin_ = origin.point;
|
||||||
|
|
||||||
|
// // // we also need to transform the cloud of the observation to the new global frame
|
||||||
|
// // // tf3_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
|
||||||
|
// // tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
|
||||||
|
// // new_global_frame, // frame đích
|
||||||
|
// // obs.cloud_->header.frame_id, // frame nguồn
|
||||||
|
// // transform_time
|
||||||
|
// // );
|
||||||
|
// // tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2);
|
||||||
|
// // }
|
||||||
|
// // catch (TransformException& ex)
|
||||||
|
// // {
|
||||||
|
// // robot::log_error("TF Error attempting to transform an observation from %s to %s: %s\n", global_frame_.c_str(),
|
||||||
|
// // new_global_frame.c_str(), ex.what());
|
||||||
|
// // return false;
|
||||||
|
// // }
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // // now we need to update our global_frame member
|
||||||
|
// // global_frame_ = new_global_frame;
|
||||||
|
// // return true;
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // // void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
|
||||||
|
// // // {
|
||||||
|
// // // robot_geometry_msgs::PointStamped global_origin;
|
||||||
|
|
||||||
|
// // // // create a new observation on the list to be populated
|
||||||
|
// // // observation_list_.push_front(Observation());
|
||||||
|
|
||||||
|
// // // // check whether the origin frame has been set explicitly or whether we should get it from the cloud
|
||||||
|
// // // string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
|
||||||
|
|
||||||
|
// // // try
|
||||||
|
// // // {
|
||||||
|
// // // // given these observations come from sensors... we'll need to store the origin pt of the sensor
|
||||||
|
// // // robot_geometry_msgs::PointStamped local_origin;
|
||||||
|
// // // local_origin.header.stamp = cloud.header.stamp;
|
||||||
|
// // // local_origin.header.frame_id = origin_frame;
|
||||||
|
// // // local_origin.point.x = 0;
|
||||||
|
// // // local_origin.point.y = 0;
|
||||||
|
// // // local_origin.point.z = 0;
|
||||||
|
// // // // tf3_buffer_.transform(local_origin, global_origin, global_frame_);
|
||||||
|
// // // tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
|
||||||
|
// // // global_frame_, // frame đích
|
||||||
|
// // // local_origin.header.frame_id, // frame nguồn
|
||||||
|
// // // tf3::Time()
|
||||||
|
// // // // data_convert::convertTime(local_origin.header.stamp)
|
||||||
|
// // // );
|
||||||
|
// // // tf3::doTransform(local_origin, global_origin, tfm_1);
|
||||||
|
// // // tf3::convert(global_origin.point, observation_list_.front().origin_);
|
||||||
|
|
||||||
|
// // // // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
|
||||||
|
// // // observation_list_.front().raytrace_range_ = raytrace_range_;
|
||||||
|
// // // observation_list_.front().obstacle_range_ = obstacle_range_;
|
||||||
|
|
||||||
|
// // // robot_sensor_msgs::PointCloud2 global_frame_cloud;
|
||||||
|
|
||||||
|
// // // // transform the point cloud
|
||||||
|
// // // // tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_);
|
||||||
|
// // // tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
|
||||||
|
// // // global_frame_, // frame đích
|
||||||
|
// // // cloud.header.frame_id, // frame nguồn
|
||||||
|
// // // tf3::Time()
|
||||||
|
// // // // data_convert::convertTime(cloud.header.stamp)
|
||||||
|
// // // );
|
||||||
|
// // // tf3::doTransform(cloud, global_frame_cloud, tfm_2);
|
||||||
|
// // // global_frame_cloud.header.stamp = cloud.header.stamp;
|
||||||
|
|
||||||
|
// // // // now we need to remove observations from the cloud that are below or above our height thresholds
|
||||||
|
// // // robot_sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
|
||||||
|
// // // observation_cloud.height = global_frame_cloud.height;
|
||||||
|
// // // observation_cloud.width = global_frame_cloud.width;
|
||||||
|
// // // observation_cloud.fields = global_frame_cloud.fields;
|
||||||
|
// // // observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
|
||||||
|
// // // observation_cloud.point_step = global_frame_cloud.point_step;
|
||||||
|
// // // observation_cloud.row_step = global_frame_cloud.row_step;
|
||||||
|
// // // observation_cloud.is_dense = global_frame_cloud.is_dense;
|
||||||
|
|
||||||
|
// // // unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
|
||||||
|
// // // robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
|
||||||
|
// // // modifier.resize(cloud_size);
|
||||||
|
// // // unsigned int point_count = 0;
|
||||||
|
|
||||||
|
// // // // copy over the points that are within our height bounds
|
||||||
|
// // // robot_sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
|
||||||
|
// // // std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
|
||||||
|
// // // std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
|
||||||
|
// // // for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
|
||||||
|
// // // {
|
||||||
|
// // // if ((*iter_z) <= max_obstacle_height_
|
||||||
|
// // // && (*iter_z) >= min_obstacle_height_)
|
||||||
|
// // // {
|
||||||
|
// // // std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
|
||||||
|
// // // iter_obs += global_frame_cloud.point_step;
|
||||||
|
// // // ++point_count;
|
||||||
|
// // // }
|
||||||
|
// // // }
|
||||||
|
|
||||||
|
// // // // resize the cloud for the number of legal points
|
||||||
|
// // // modifier.resize(point_count);
|
||||||
|
// // // observation_cloud.header.stamp = cloud.header.stamp;
|
||||||
|
// // // observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
|
||||||
|
// // // }
|
||||||
|
// // // catch (TransformException& ex)
|
||||||
|
// // // {
|
||||||
|
// // // // if an exception occurs, we need to remove the empty observation from the list
|
||||||
|
// // // observation_list_.pop_front();
|
||||||
|
// // // robot::log_error("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
|
||||||
|
// // // cloud.header.frame_id.c_str(), ex.what());
|
||||||
|
// // // return;
|
||||||
|
// // // }
|
||||||
|
|
||||||
|
// // // // if the update was successful, we want to update the last updated time
|
||||||
|
// // // last_updated_ = robot::Time::now();
|
||||||
|
|
||||||
|
// // // // we'll also remove any stale observations from the list
|
||||||
|
// // // purgeStaleObservations();
|
||||||
|
// // // }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// // // void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
|
||||||
|
// // // {
|
||||||
|
// // // robot_geometry_msgs::PointStamped global_origin;
|
||||||
|
|
||||||
|
// // // observation_list_.push_front(Observation());
|
||||||
|
|
||||||
|
// // // string origin_frame = sensor_frame_.empty() ? cloud.header.frame_id : sensor_frame_;
|
||||||
|
|
||||||
|
// // // try
|
||||||
|
// // // {
|
||||||
|
// // // // --- [1] TF lookups: giữ nguyên, không thể tránh ---
|
||||||
|
// // // robot_geometry_msgs::PointStamped local_origin;
|
||||||
|
// // // local_origin.header.stamp = cloud.header.stamp;
|
||||||
|
// // // local_origin.header.frame_id = origin_frame;
|
||||||
|
// // // local_origin.point.x = local_origin.point.y = local_origin.point.z = 0;
|
||||||
|
|
||||||
|
// // // tf3::TransformStampedMsg tfm_origin = tf3_buffer_.lookupTransform(
|
||||||
|
// // // global_frame_, local_origin.header.frame_id, tf3::Time());
|
||||||
|
// // // tf3::doTransform(local_origin, global_origin, tfm_origin);
|
||||||
|
// // // // origin là 1 điểm duy nhất → doTransform ổn
|
||||||
|
// // // tf3::convert(global_origin.point, observation_list_.front().origin_);
|
||||||
|
|
||||||
|
// // // observation_list_.front().raytrace_range_ = raytrace_range_;
|
||||||
|
// // // observation_list_.front().obstacle_range_ = obstacle_range_;
|
||||||
|
|
||||||
|
// // // // --- [2] Trích ma trận transform 1 lần cho toàn bộ cloud ---
|
||||||
|
// // // tf3::TransformStampedMsg tfm_cloud = tf3_buffer_.lookupTransform(
|
||||||
|
// // // global_frame_, cloud.header.frame_id, tf3::Time());
|
||||||
|
// // // const Transform4x4 M = extractMatrix(tfm_cloud);
|
||||||
|
|
||||||
|
// // // // --- [3] Tìm offset của x, y, z trong point layout ---
|
||||||
|
// // // int x_off = -1, y_off = -1, z_off = -1;
|
||||||
|
// // // for (const auto& field : cloud.fields) {
|
||||||
|
// // // if (field.name == "x") x_off = (int)field.offset;
|
||||||
|
// // // else if (field.name == "y") y_off = (int)field.offset;
|
||||||
|
// // // else if (field.name == "z") z_off = (int)field.offset;
|
||||||
|
// // // }
|
||||||
|
// // // // Fallback nếu không tìm thấy (không nên xảy ra với PointCloud2 hợp lệ)
|
||||||
|
// // // if (x_off < 0 || y_off < 0 || z_off < 0) {
|
||||||
|
// // // observation_list_.pop_front();
|
||||||
|
// // // robot::log_error("PointCloud2 thiếu field x/y/z\n");
|
||||||
|
// // // return;
|
||||||
|
// // // }
|
||||||
|
|
||||||
|
// // // // --- [4] Setup output cloud (copy metadata, không copy data) ---
|
||||||
|
// // // robot_sensor_msgs::PointCloud2& obs_cloud = *(observation_list_.front().cloud_);
|
||||||
|
// // // obs_cloud.fields = cloud.fields; // shallow copy, thường nhỏ
|
||||||
|
// // // obs_cloud.is_bigendian = cloud.is_bigendian;
|
||||||
|
// // // obs_cloud.point_step = cloud.point_step;
|
||||||
|
// // // obs_cloud.is_dense = cloud.is_dense;
|
||||||
|
// // // obs_cloud.height = 1; // output luôn là unordered
|
||||||
|
|
||||||
|
// // // const uint32_t point_step = cloud.point_step;
|
||||||
|
// // // const uint32_t cloud_size = cloud.height * cloud.width;
|
||||||
|
|
||||||
|
// // // // [KEY] Reserve trước toàn bộ capacity → tránh realloc nhiều lần
|
||||||
|
// // // // Worst case: tất cả points đều pass filter
|
||||||
|
// // // obs_cloud.data.reserve(static_cast<size_t>(cloud_size) * point_step);
|
||||||
|
|
||||||
|
// // // // --- [5] SINGLE PASS: transform + filter + copy ---
|
||||||
|
// // // const uint8_t* src_ptr = cloud.data.data();
|
||||||
|
// // // uint32_t point_count = 0;
|
||||||
|
|
||||||
|
// // // for (uint32_t i = 0; i < cloud_size; ++i, src_ptr += point_step)
|
||||||
|
// // // {
|
||||||
|
// // // // Đọc x, y, z gốc (float32)
|
||||||
|
// // // float lx, ly, lz;
|
||||||
|
// // // std::memcpy(&lx, src_ptr + x_off, sizeof(float));
|
||||||
|
// // // std::memcpy(&ly, src_ptr + y_off, sizeof(float));
|
||||||
|
// // // std::memcpy(&lz, src_ptr + z_off, sizeof(float));
|
||||||
|
|
||||||
|
// // // // Áp dụng transform (matrix multiply inline, không có virtual call)
|
||||||
|
// // // // Với PointCloud2 thường dùng float32, cast double→float ở cuối
|
||||||
|
// // // const double gx = M.m[0][0]*lx + M.m[0][1]*ly + M.m[0][2]*lz + M.m[0][3];
|
||||||
|
// // // const double gy = M.m[1][0]*lx + M.m[1][1]*ly + M.m[1][2]*lz + M.m[1][3];
|
||||||
|
// // // const double gz = M.m[2][0]*lx + M.m[2][1]*ly + M.m[2][2]*lz + M.m[2][3];
|
||||||
|
|
||||||
|
// // // // Filter height (dùng gz vừa tính, không cần PointCloud2Iterator)
|
||||||
|
// // // if (gz < min_obstacle_height_ || gz > max_obstacle_height_)
|
||||||
|
// // // continue;
|
||||||
|
|
||||||
|
// // // // Copy toàn bộ point (giữ nguyên các field khác: intensity, ring, …)
|
||||||
|
// // // // rồi patch lại x, y, z bằng giá trị đã transform
|
||||||
|
// // // const size_t insert_pos = obs_cloud.data.size();
|
||||||
|
// // // obs_cloud.data.resize(insert_pos + point_step);
|
||||||
|
// // // uint8_t* dst_ptr = obs_cloud.data.data() + insert_pos;
|
||||||
|
|
||||||
|
// // // std::memcpy(dst_ptr, src_ptr, point_step);
|
||||||
|
|
||||||
|
// // // // Ghi lại x, y, z đã transform (float32)
|
||||||
|
// // // const float gxf = static_cast<float>(gx);
|
||||||
|
// // // const float gyf = static_cast<float>(gy);
|
||||||
|
// // // const float gzf = static_cast<float>(gz);
|
||||||
|
// // // std::memcpy(dst_ptr + x_off, &gxf, sizeof(float));
|
||||||
|
// // // std::memcpy(dst_ptr + y_off, &gyf, sizeof(float));
|
||||||
|
// // // std::memcpy(dst_ptr + z_off, &gzf, sizeof(float));
|
||||||
|
|
||||||
|
// // // ++point_count;
|
||||||
|
// // // }
|
||||||
|
|
||||||
|
// // // // --- [6] Finalize output ---
|
||||||
|
// // // obs_cloud.width = point_count;
|
||||||
|
// // // obs_cloud.row_step = point_count * point_step;
|
||||||
|
// // // obs_cloud.header.stamp = cloud.header.stamp;
|
||||||
|
// // // obs_cloud.header.frame_id = global_frame_;
|
||||||
|
|
||||||
|
// // // // Giải phóng capacity dư (optional, tùy memory pressure)
|
||||||
|
// // // // obs_cloud.data.shrink_to_fit();
|
||||||
|
// // // }
|
||||||
|
// // // catch (TransformException& ex)
|
||||||
|
// // // {
|
||||||
|
// // // observation_list_.pop_front();
|
||||||
|
// // // robot::log_error("TF Exception: sensor_frame=%s, cloud_frame=%s: %s\n",
|
||||||
|
// // // sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what());
|
||||||
|
// // // return;
|
||||||
|
// // // }
|
||||||
|
|
||||||
|
// // // last_updated_ = robot::Time::now();
|
||||||
|
// // // purgeStaleObservations();
|
||||||
|
// // // }
|
||||||
|
|
||||||
|
// // void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
|
||||||
|
// // {
|
||||||
|
// // robot_geometry_msgs::PointStamped global_origin;
|
||||||
|
// // observation_list_.push_front(Observation());
|
||||||
|
// // string origin_frame = sensor_frame_.empty() ? cloud.header.frame_id : sensor_frame_;
|
||||||
|
|
||||||
|
// // try
|
||||||
|
// // {
|
||||||
|
// // // [1] Transform origin (giữ nguyên)
|
||||||
|
// // robot_geometry_msgs::PointStamped local_origin;
|
||||||
|
// // local_origin.header.stamp = cloud.header.stamp;
|
||||||
|
// // local_origin.header.frame_id = origin_frame;
|
||||||
|
// // local_origin.point.x = local_origin.point.y = local_origin.point.z = 0;
|
||||||
|
|
||||||
|
// // tf3::TransformStampedMsg tfm_origin = tf3_buffer_.lookupTransform(
|
||||||
|
// // global_frame_, local_origin.header.frame_id, tf3::Time());
|
||||||
|
// // tf3::doTransform(local_origin, global_origin, tfm_origin);
|
||||||
|
// // tf3::convert(global_origin.point, observation_list_.front().origin_);
|
||||||
|
|
||||||
|
// // observation_list_.front().raytrace_range_ = raytrace_range_;
|
||||||
|
// // observation_list_.front().obstacle_range_ = obstacle_range_;
|
||||||
|
|
||||||
|
// // // [2] Lấy transform matrix 1 lần
|
||||||
|
// // tf3::TransformStampedMsg tfm_cloud = tf3_buffer_.lookupTransform(
|
||||||
|
// // global_frame_, cloud.header.frame_id, tf3::Time());
|
||||||
|
// // const Transform4x4 M = extractMatrix(tfm_cloud);
|
||||||
|
|
||||||
|
// // // [3] Tìm offset x/y/z
|
||||||
|
// // int x_off = -1, y_off = -1, z_off = -1;
|
||||||
|
// // for (const auto& field : cloud.fields) {
|
||||||
|
// // if (field.name == "x") x_off = (int)field.offset;
|
||||||
|
// // else if (field.name == "y") y_off = (int)field.offset;
|
||||||
|
// // else if (field.name == "z") z_off = (int)field.offset;
|
||||||
|
// // }
|
||||||
|
// // if (x_off < 0 || y_off < 0 || z_off < 0) {
|
||||||
|
// // observation_list_.pop_front();
|
||||||
|
// // robot::log_error("PointCloud2 thiếu field x/y/z\n");
|
||||||
|
// // return;
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // // [4] Setup output cloud
|
||||||
|
// // robot_sensor_msgs::PointCloud2& obs_cloud = *(observation_list_.front().cloud_);
|
||||||
|
// // obs_cloud.fields = cloud.fields;
|
||||||
|
// // obs_cloud.is_bigendian = cloud.is_bigendian;
|
||||||
|
// // obs_cloud.point_step = cloud.point_step;
|
||||||
|
// // obs_cloud.is_dense = cloud.is_dense;
|
||||||
|
// // obs_cloud.height = 1;
|
||||||
|
|
||||||
|
// // const uint32_t point_step = cloud.point_step;
|
||||||
|
// // const uint32_t cloud_size = cloud.height * cloud.width;
|
||||||
|
|
||||||
|
// // // ─────────────────────────────────────────────────────────────────
|
||||||
|
// // // [5] VOXEL FILTER theo cell costmap
|
||||||
|
// // //
|
||||||
|
// // // Thay vì giữ 6.5M points, ta hash mỗi point về (voxel_x, voxel_y)
|
||||||
|
// // // theo voxel_size = costmap resolution (thường 0.05m).
|
||||||
|
// // // Mỗi voxel cell chỉ giữ 1 point đại diện (first hit).
|
||||||
|
// // //
|
||||||
|
// // // Kết quả: 6.5M → số lượng ô costmap thực sự có obstacle
|
||||||
|
// // // (~vài nghìn đến vài chục nghìn, tùy scene)
|
||||||
|
// // // ─────────────────────────────────────────────────────────────────
|
||||||
|
// // const double voxel_size = voxel_size_; // khớp với costmap resolution
|
||||||
|
// // const double inv_voxel_size = 1.0 / voxel_size;
|
||||||
|
|
||||||
|
// // // unordered_map: key = packed (ix, iy) → value = raw point bytes
|
||||||
|
// // // Dùng int64 pack để tránh custom hash
|
||||||
|
// // struct VoxelData {
|
||||||
|
// // float x, y, z;
|
||||||
|
// // std::vector<uint8_t> raw; // toàn bộ point_step bytes gốc
|
||||||
|
// // };
|
||||||
|
|
||||||
|
// // // Ước lượng số voxel thực tế: reserve để tránh rehash
|
||||||
|
// // // Với scene thực tế thường << 100K cells có obstacle
|
||||||
|
// // std::unordered_map<int64_t, VoxelData> voxel_map;
|
||||||
|
// // voxel_map.reserve(65536); // 64K slots ban đầu
|
||||||
|
|
||||||
|
// // const uint8_t* src_ptr = cloud.data.data();
|
||||||
|
|
||||||
|
// // for (uint32_t i = 0; i < cloud_size; ++i, src_ptr += point_step)
|
||||||
|
// // {
|
||||||
|
// // float lx, ly, lz;
|
||||||
|
// // std::memcpy(&lx, src_ptr + x_off, sizeof(float));
|
||||||
|
// // std::memcpy(&ly, src_ptr + y_off, sizeof(float));
|
||||||
|
// // std::memcpy(&lz, src_ptr + z_off, sizeof(float));
|
||||||
|
|
||||||
|
// // // Bỏ qua NaN (thường xuất hiện trong depth camera cloud)
|
||||||
|
// // if (!std::isfinite(lx) || !std::isfinite(ly) || !std::isfinite(lz))
|
||||||
|
// // continue;
|
||||||
|
|
||||||
|
// // // Transform sang global frame
|
||||||
|
// // const double gx = M.m[0][0]*lx + M.m[0][1]*ly + M.m[0][2]*lz + M.m[0][3];
|
||||||
|
// // const double gy = M.m[1][0]*lx + M.m[1][1]*ly + M.m[1][2]*lz + M.m[1][3];
|
||||||
|
// // const double gz = M.m[2][0]*lx + M.m[2][1]*ly + M.m[2][2]*lz + M.m[2][3];
|
||||||
|
|
||||||
|
// // // Height filter
|
||||||
|
// // if (gz < min_obstacle_height_ || gz > max_obstacle_height_)
|
||||||
|
// // continue;
|
||||||
|
|
||||||
|
// // // Tính voxel index (floor division, handle negative coords)
|
||||||
|
// // const int32_t ix = static_cast<int32_t>(std::floor(gx * inv_voxel_size));
|
||||||
|
// // const int32_t iy = static_cast<int32_t>(std::floor(gy * inv_voxel_size));
|
||||||
|
|
||||||
|
// // // Pack 2×int32 thành 1×int64 làm key
|
||||||
|
// // const int64_t key = (static_cast<int64_t>(ix) << 32) |
|
||||||
|
// // static_cast<int64_t>(static_cast<uint32_t>(iy));
|
||||||
|
|
||||||
|
// // // Chỉ insert nếu voxel này chưa có điểm nào (first-hit policy)
|
||||||
|
// // auto result = voxel_map.emplace(key, VoxelData{});
|
||||||
|
// // if (result.second) // true = voxel mới, chưa có data
|
||||||
|
// // {
|
||||||
|
// // VoxelData& vd = result.first->second;
|
||||||
|
// // vd.x = static_cast<float>(gx);
|
||||||
|
// // vd.y = static_cast<float>(gy);
|
||||||
|
// // vd.z = static_cast<float>(gz);
|
||||||
|
// // vd.raw.assign(src_ptr, src_ptr + point_step);
|
||||||
|
// // // Patch x/y/z trong raw bytes ngay tại đây
|
||||||
|
// // std::memcpy(vd.raw.data() + x_off, &vd.x, sizeof(float));
|
||||||
|
// // std::memcpy(vd.raw.data() + y_off, &vd.y, sizeof(float));
|
||||||
|
// // std::memcpy(vd.raw.data() + z_off, &vd.z, sizeof(float));
|
||||||
|
// // }
|
||||||
|
// // // Nếu voxel đã có → bỏ qua (không cần xử lý thêm)
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // // [6] Ghi kết quả voxel filter vào obs_cloud
|
||||||
|
// // const uint32_t point_count = static_cast<uint32_t>(voxel_map.size());
|
||||||
|
// // obs_cloud.data.resize(static_cast<size_t>(point_count) * point_step);
|
||||||
|
|
||||||
|
// // uint8_t* dst = obs_cloud.data.data();
|
||||||
|
// // for (const auto& kv : voxel_map)
|
||||||
|
// // {
|
||||||
|
// // std::memcpy(dst, kv.second.raw.data(), point_step);
|
||||||
|
// // dst += point_step;
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // obs_cloud.width = point_count;
|
||||||
|
// // obs_cloud.row_step = point_count * point_step;
|
||||||
|
// // obs_cloud.header.stamp = cloud.header.stamp;
|
||||||
|
// // obs_cloud.header.frame_id = global_frame_;
|
||||||
|
// // }
|
||||||
|
// // catch (TransformException& ex)
|
||||||
|
// // {
|
||||||
|
// // observation_list_.pop_front();
|
||||||
|
// // robot::log_error("TF Exception: sensor_frame=%s, cloud_frame=%s: %s\n",
|
||||||
|
// // sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what());
|
||||||
|
// // return;
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // last_updated_ = robot::Time::now();
|
||||||
|
// // purgeStaleObservations();
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // // returns a copy of the observations
|
||||||
|
// // void ObservationBuffer::getObservations(vector<Observation>& observations)
|
||||||
|
// // {
|
||||||
|
// // // first... let's make sure that we don't have any stale observations
|
||||||
|
// // purgeStaleObservations();
|
||||||
|
|
||||||
|
// // // now we'll just copy the observations for the caller
|
||||||
|
// // list<Observation>::iterator obs_it;
|
||||||
|
// // for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
|
||||||
|
// // {
|
||||||
|
// // observations.push_back(*obs_it);
|
||||||
|
// // }
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // void ObservationBuffer::purgeStaleObservations()
|
||||||
|
// // {
|
||||||
|
// // if (!observation_list_.empty())
|
||||||
|
// // {
|
||||||
|
// // list<Observation>::iterator obs_it = observation_list_.begin();
|
||||||
|
// // // if we're keeping observations for no time... then we'll only keep one observation
|
||||||
|
// // if (observation_keep_time_ == robot::Duration(0.0))
|
||||||
|
// // {
|
||||||
|
// // observation_list_.erase(++obs_it, observation_list_.end());
|
||||||
|
// // return;
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // // otherwise... we'll have to loop through the observations to see which ones are stale
|
||||||
|
// // for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
|
||||||
|
// // {
|
||||||
|
// // Observation& obs = *obs_it;
|
||||||
|
// // // check if the observation is out of date... and if it is, remove it and those that follow from the list
|
||||||
|
// // if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
|
||||||
|
// // {
|
||||||
|
// // observation_list_.erase(obs_it, observation_list_.end());
|
||||||
|
// // return;
|
||||||
|
// // }
|
||||||
|
// // }
|
||||||
|
// // }
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // bool ObservationBuffer::isCurrent() const
|
||||||
|
// // {
|
||||||
|
// // if (expected_update_rate_ == robot::Duration(0.0))
|
||||||
|
// // return true;
|
||||||
|
|
||||||
|
// // bool current = (robot::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
|
||||||
|
// // if (!current)
|
||||||
|
// // {
|
||||||
|
// // robot::log_error("The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
|
||||||
|
// // topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
|
||||||
|
// // }
|
||||||
|
// // return current;
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// // void ObservationBuffer::resetLastUpdated()
|
||||||
|
// // {
|
||||||
|
// // last_updated_ = robot::Time::now();
|
||||||
|
// // }
|
||||||
|
// // } // namespace robot_costmap_2d
|
||||||
|
|
||||||
|
// /*********************************************************************
|
||||||
|
// *
|
||||||
|
// * Software License Agreement (BSD License)
|
||||||
|
// *
|
||||||
|
// * Copyright (c) 2008, 2013, Willow Garage, Inc.
|
||||||
|
// * All rights reserved.
|
||||||
|
// * (License text omitted for brevity – same as original)
|
||||||
|
// *
|
||||||
|
// * Author: Eitan Marder-Eppstein
|
||||||
|
// *
|
||||||
|
// * ── Optimization notes ──────────────────────────────────────────────
|
||||||
|
// *
|
||||||
|
// * bufferCloud() – single-pass voxel-downsampling transform
|
||||||
|
// * ─────────────────────────────────────────────────────────────────────
|
||||||
|
// * Original pipeline (3 passes, 6.5 M × point_step bytes each):
|
||||||
|
// * Pass 1 – tf3::doTransform → global_frame_cloud (new allocation)
|
||||||
|
// * Pass 2 – height filter → observation_cloud (byte-by-byte copy)
|
||||||
|
// * Pass 3 – updateBounds/raytrace iter the result again
|
||||||
|
// *
|
||||||
|
// * Optimised pipeline (1 pass, result << 6.5 M points):
|
||||||
|
// * Single loop:
|
||||||
|
// * memcpy x/y/z → inline matrix-multiply → height filter →
|
||||||
|
// * voxel-hash (int64 key) → first-hit insert into flat output buffer
|
||||||
|
// *
|
||||||
|
// * Voxel size = costmap resolution (default 0.05 m).
|
||||||
|
// * With a 10 m × 10 m map → at most 40 000 output points instead of
|
||||||
|
// * 6 500 000. Every downstream consumer (updateBounds marking loop,
|
||||||
|
// * raytraceFreespace, frustum-clearing FOV scan) benefits equally.
|
||||||
|
// *
|
||||||
|
// * Key micro-optimisations
|
||||||
|
// * ───────────────────────
|
||||||
|
// * • extractMatrix() – quaternion → 4×4 double once per cloud; avoids
|
||||||
|
// * virtual-dispatch / exception-guard overhead of doTransform per point.
|
||||||
|
// * • std::memcpy for unaligned float reads (safe on all platforms).
|
||||||
|
// * • unordered_map::emplace with int64 packed key – O(1) amortised.
|
||||||
|
// * • reserve(65536) on the map to avoid rehash for typical scenes.
|
||||||
|
// * • Output data written directly into obs_cloud.data (no intermediate
|
||||||
|
// * vector of VoxelData structs on heap).
|
||||||
|
// * • NaN guard before the hash (depth cameras emit NaN for invalid pixels).
|
||||||
|
// *********************************************************************/
|
||||||
|
// #include <robot_costmap_2d/observation_buffer.h>
|
||||||
|
|
||||||
|
// #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||||
|
// #include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
|
||||||
|
// #include <robot_sensor_msgs/point_cloud2_iterator.h>
|
||||||
|
|
||||||
|
// #include <unordered_map>
|
||||||
|
// #include <cmath>
|
||||||
|
// #include <cstring>
|
||||||
|
|
||||||
|
// using namespace std;
|
||||||
|
// using namespace tf3;
|
||||||
|
|
||||||
|
// namespace robot_costmap_2d
|
||||||
|
// {
|
||||||
|
|
||||||
|
// // ── Constructor / Destructor ─────────────────────────────────────────────────
|
||||||
|
|
||||||
|
// ObservationBuffer::ObservationBuffer(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,
|
||||||
|
// string global_frame,
|
||||||
|
// string sensor_frame,
|
||||||
|
// double tf_tolerance)
|
||||||
|
// : tf3_buffer_(tf3_buffer)
|
||||||
|
// , observation_keep_time_(observation_keep_time)
|
||||||
|
// , expected_update_rate_(expected_update_rate)
|
||||||
|
// , last_updated_(robot::Time::now())
|
||||||
|
// , global_frame_(global_frame)
|
||||||
|
// , sensor_frame_(sensor_frame)
|
||||||
|
// , topic_name_(topic_name)
|
||||||
|
// , min_obstacle_height_(min_obstacle_height)
|
||||||
|
// , max_obstacle_height_(max_obstacle_height)
|
||||||
|
// , obstacle_range_(obstacle_range)
|
||||||
|
// , raytrace_range_(raytrace_range)
|
||||||
|
// , tf_tolerance_(tf_tolerance)
|
||||||
|
// , voxel_size_(0.05)
|
||||||
|
// , inv_voxel_size_(20.0)
|
||||||
|
// {}
|
||||||
|
|
||||||
|
// ObservationBuffer::~ObservationBuffer() {}
|
||||||
|
|
||||||
|
// // ── setGlobalFrame ───────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
// bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
|
||||||
|
// {
|
||||||
|
// tf3::Time transform_time = tf3::Time::now();
|
||||||
|
// std::string tf_error;
|
||||||
|
|
||||||
|
// if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
|
||||||
|
// {
|
||||||
|
// robot::log_error("Transform between %s and %s with tolerance %.2f failed: %s.\n",
|
||||||
|
// new_global_frame.c_str(), global_frame_.c_str(),
|
||||||
|
// tf_tolerance_, tf_error.c_str());
|
||||||
|
// return false;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// for (auto& obs : observation_list_)
|
||||||
|
// {
|
||||||
|
// try
|
||||||
|
// {
|
||||||
|
// robot_geometry_msgs::PointStamped origin;
|
||||||
|
// origin.header.frame_id = global_frame_;
|
||||||
|
// origin.header.stamp = data_convert::convertTime(transform_time);
|
||||||
|
// origin.point = obs.origin_;
|
||||||
|
|
||||||
|
// tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
|
||||||
|
// new_global_frame, origin.header.frame_id, transform_time);
|
||||||
|
// tf3::doTransform(origin, origin, tfm_1);
|
||||||
|
// obs.origin_ = origin.point;
|
||||||
|
|
||||||
|
// tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
|
||||||
|
// new_global_frame, obs.cloud_->header.frame_id, transform_time);
|
||||||
|
// tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2);
|
||||||
|
// }
|
||||||
|
// catch (TransformException& ex)
|
||||||
|
// {
|
||||||
|
// robot::log_error("TF Error attempting to transform an observation from %s to %s: %s\n",
|
||||||
|
// global_frame_.c_str(), new_global_frame.c_str(), ex.what());
|
||||||
|
// return false;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// global_frame_ = new_global_frame;
|
||||||
|
// return true;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // ── bufferCloud ──────────────────────────────────────────────────────────────
|
||||||
|
// //
|
||||||
|
// // Single-pass: transform → height-filter → voxel-downsample → write output.
|
||||||
|
// // No intermediate global_frame_cloud allocation.
|
||||||
|
|
||||||
|
// void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
|
||||||
|
// {
|
||||||
|
// robot_geometry_msgs::PointStamped global_origin;
|
||||||
|
// observation_list_.push_front(Observation());
|
||||||
|
|
||||||
|
// const string origin_frame = sensor_frame_.empty() ? cloud.header.frame_id : sensor_frame_;
|
||||||
|
|
||||||
|
// try
|
||||||
|
// {
|
||||||
|
// // ── [1] Transform sensor origin (single point – doTransform is fine) ──
|
||||||
|
// robot_geometry_msgs::PointStamped local_origin;
|
||||||
|
// local_origin.header.stamp = cloud.header.stamp;
|
||||||
|
// local_origin.header.frame_id = origin_frame;
|
||||||
|
// local_origin.point.x = local_origin.point.y = local_origin.point.z = 0.0;
|
||||||
|
|
||||||
|
// tf3::TransformStampedMsg tfm_origin =
|
||||||
|
// tf3_buffer_.lookupTransform(global_frame_, origin_frame, tf3::Time());
|
||||||
|
// tf3::doTransform(local_origin, global_origin, tfm_origin);
|
||||||
|
// tf3::convert(global_origin.point, observation_list_.front().origin_);
|
||||||
|
|
||||||
|
// observation_list_.front().raytrace_range_ = raytrace_range_;
|
||||||
|
// observation_list_.front().obstacle_range_ = obstacle_range_;
|
||||||
|
|
||||||
|
// // ── [2] Extract 4×4 rotation+translation matrix once ─────────────────
|
||||||
|
// tf3::TransformStampedMsg tfm_cloud =
|
||||||
|
// tf3_buffer_.lookupTransform(global_frame_, cloud.header.frame_id, tf3::Time());
|
||||||
|
// const Transform4x4 M = extractMatrix(tfm_cloud);
|
||||||
|
|
||||||
|
// // ── [3] Find byte offsets of x, y, z fields ───────────────────────────
|
||||||
|
// int x_off = -1, y_off = -1, z_off = -1;
|
||||||
|
// for (const auto& f : cloud.fields)
|
||||||
|
// {
|
||||||
|
// if (f.name == "x") x_off = static_cast<int>(f.offset);
|
||||||
|
// else if (f.name == "y") y_off = static_cast<int>(f.offset);
|
||||||
|
// else if (f.name == "z") z_off = static_cast<int>(f.offset);
|
||||||
|
// }
|
||||||
|
// if (x_off < 0 || y_off < 0 || z_off < 0)
|
||||||
|
// {
|
||||||
|
// observation_list_.pop_front();
|
||||||
|
// robot::log_error("ObservationBuffer::bufferCloud – PointCloud2 missing x/y/z fields\n");
|
||||||
|
// return;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // ── [4] Prepare output cloud metadata (no data copy yet) ─────────────
|
||||||
|
// robot_sensor_msgs::PointCloud2& obs_cloud = *(observation_list_.front().cloud_);
|
||||||
|
// obs_cloud.fields = cloud.fields;
|
||||||
|
// obs_cloud.is_bigendian = cloud.is_bigendian;
|
||||||
|
// obs_cloud.point_step = cloud.point_step;
|
||||||
|
// obs_cloud.is_dense = cloud.is_dense;
|
||||||
|
// obs_cloud.height = 1; // unordered output
|
||||||
|
|
||||||
|
// const uint32_t point_step = cloud.point_step;
|
||||||
|
// const uint32_t cloud_size = cloud.height * cloud.width;
|
||||||
|
|
||||||
|
// // ── [5] Voxel-grid downsampling + transform + height filter (1 pass) ──
|
||||||
|
// //
|
||||||
|
// // Key insight for 2-D costmap:
|
||||||
|
// // Two points that fall in the same (ix, iy) voxel cell will mark the
|
||||||
|
// // same costmap cell, so we only need one representative per voxel.
|
||||||
|
// // We use first-hit policy: whichever point is encountered first wins.
|
||||||
|
// //
|
||||||
|
// // Hash: pack (int32_t ix, int32_t iy) → int64_t key.
|
||||||
|
// // • No custom hasher needed (default hash<int64_t> is fast).
|
||||||
|
// // • Negative world coordinates are handled correctly by casting
|
||||||
|
// // int32 → uint32 before the shift.
|
||||||
|
|
||||||
|
// // Preallocate output buffer worst-case (all points pass filter).
|
||||||
|
// // In practice the voxel map will be much smaller; we'll resize at end.
|
||||||
|
// obs_cloud.data.reserve(static_cast<size_t>(cloud_size) * point_step);
|
||||||
|
|
||||||
|
// // Voxel map: key → byte offset in obs_cloud.data (first-hit written directly)
|
||||||
|
// std::unordered_map<int64_t, uint32_t> voxel_map;
|
||||||
|
// voxel_map.reserve(65536); // 64 K buckets – covers typical indoor scenes
|
||||||
|
|
||||||
|
// const double inv_vs = inv_voxel_size_;
|
||||||
|
// const double min_h = min_obstacle_height_;
|
||||||
|
// const double max_h = max_obstacle_height_;
|
||||||
|
// const uint8_t* src = cloud.data.data();
|
||||||
|
// uint32_t point_count = 0;
|
||||||
|
|
||||||
|
// for (uint32_t i = 0; i < cloud_size; ++i, src += point_step)
|
||||||
|
// {
|
||||||
|
// // Read local x/y/z (float32, potentially unaligned)
|
||||||
|
// float lx, ly, lz;
|
||||||
|
// std::memcpy(&lx, src + x_off, sizeof(float));
|
||||||
|
// std::memcpy(&ly, src + y_off, sizeof(float));
|
||||||
|
// std::memcpy(&lz, src + z_off, sizeof(float));
|
||||||
|
|
||||||
|
// // Skip NaN / Inf (common in depth camera output)
|
||||||
|
// if (!std::isfinite(lx) || !std::isfinite(ly) || !std::isfinite(lz))
|
||||||
|
// continue;
|
||||||
|
|
||||||
|
// // Inline 3-D transform: gp = M * [lx, ly, lz, 1]^T
|
||||||
|
// const double gx = M.m[0][0]*lx + M.m[0][1]*ly + M.m[0][2]*lz + M.m[0][3];
|
||||||
|
// const double gy = M.m[1][0]*lx + M.m[1][1]*ly + M.m[1][2]*lz + M.m[1][3];
|
||||||
|
// const double gz = M.m[2][0]*lx + M.m[2][1]*ly + M.m[2][2]*lz + M.m[2][3];
|
||||||
|
|
||||||
|
// // Height filter (in global frame)
|
||||||
|
// if (gz < min_h || gz > max_h)
|
||||||
|
// continue;
|
||||||
|
|
||||||
|
// // Voxel index (floor division – correct for negative coordinates)
|
||||||
|
// const int32_t ix = static_cast<int32_t>(std::floor(gx * inv_vs));
|
||||||
|
// const int32_t iy = static_cast<int32_t>(std::floor(gy * inv_vs));
|
||||||
|
|
||||||
|
// // Pack into single int64 key
|
||||||
|
// const int64_t key =
|
||||||
|
// (static_cast<int64_t>(ix) << 32) |
|
||||||
|
// static_cast<int64_t>(static_cast<uint32_t>(iy));
|
||||||
|
|
||||||
|
// // Try to insert; skip if this voxel already has a representative
|
||||||
|
// if (!voxel_map.emplace(key, point_count).second)
|
||||||
|
// continue;
|
||||||
|
|
||||||
|
// // Write point into output buffer
|
||||||
|
// const size_t insert_pos = obs_cloud.data.size();
|
||||||
|
// obs_cloud.data.resize(insert_pos + point_step);
|
||||||
|
// uint8_t* dst = obs_cloud.data.data() + insert_pos;
|
||||||
|
|
||||||
|
// std::memcpy(dst, src, point_step);
|
||||||
|
|
||||||
|
// // Patch x/y/z with transformed (global-frame) values
|
||||||
|
// const float gxf = static_cast<float>(gx);
|
||||||
|
// const float gyf = static_cast<float>(gy);
|
||||||
|
// const float gzf = static_cast<float>(gz);
|
||||||
|
// std::memcpy(dst + x_off, &gxf, sizeof(float));
|
||||||
|
// std::memcpy(dst + y_off, &gyf, sizeof(float));
|
||||||
|
// std::memcpy(dst + z_off, &gzf, sizeof(float));
|
||||||
|
|
||||||
|
// ++point_count;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // ── [6] Finalise output cloud ─────────────────────────────────────────
|
||||||
|
// obs_cloud.width = point_count;
|
||||||
|
// obs_cloud.row_step = point_count * point_step;
|
||||||
|
// obs_cloud.header.stamp = cloud.header.stamp;
|
||||||
|
// obs_cloud.header.frame_id = global_frame_;
|
||||||
|
// }
|
||||||
|
// catch (TransformException& ex)
|
||||||
|
// {
|
||||||
|
// observation_list_.pop_front();
|
||||||
|
// robot::log_error("TF Exception in bufferCloud – sensor_frame=%s, cloud_frame=%s: %s\n",
|
||||||
|
// sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what());
|
||||||
|
// return;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// last_updated_ = robot::Time::now();
|
||||||
|
// purgeStaleObservations();
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // ── getObservations ──────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
// void ObservationBuffer::getObservations(vector<Observation>& observations)
|
||||||
|
// {
|
||||||
|
// purgeStaleObservations();
|
||||||
|
|
||||||
|
// for (const auto& obs : observation_list_)
|
||||||
|
// observations.push_back(obs);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // ── purgeStaleObservations ───────────────────────────────────────────────────
|
||||||
|
|
||||||
|
// void ObservationBuffer::purgeStaleObservations()
|
||||||
|
// {
|
||||||
|
// if (observation_list_.empty())
|
||||||
|
// return;
|
||||||
|
|
||||||
|
// // If keep_time == 0 → keep only the most recent observation
|
||||||
|
// if (observation_keep_time_ == robot::Duration(0.0))
|
||||||
|
// {
|
||||||
|
// auto it = observation_list_.begin();
|
||||||
|
// observation_list_.erase(++it, observation_list_.end());
|
||||||
|
// return;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // Walk forward and erase from first stale entry onward
|
||||||
|
// for (auto it = observation_list_.begin(); it != observation_list_.end(); ++it)
|
||||||
|
// {
|
||||||
|
// if ((last_updated_ - it->cloud_->header.stamp) > observation_keep_time_)
|
||||||
|
// {
|
||||||
|
// observation_list_.erase(it, observation_list_.end());
|
||||||
|
// return;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // ── isCurrent ────────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
// bool ObservationBuffer::isCurrent() const
|
||||||
|
// {
|
||||||
|
// if (expected_update_rate_ == robot::Duration(0.0))
|
||||||
|
// return true;
|
||||||
|
|
||||||
|
// const bool current =
|
||||||
|
// (robot::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
|
||||||
|
|
||||||
|
// if (!current)
|
||||||
|
// {
|
||||||
|
// robot::log_error(
|
||||||
|
// "The %s observation buffer has not been updated for %.2f seconds, "
|
||||||
|
// "and it should be updated every %.2f seconds.\n",
|
||||||
|
// topic_name_.c_str(),
|
||||||
|
// (robot::Time::now() - last_updated_).toSec(),
|
||||||
|
// expected_update_rate_.toSec());
|
||||||
|
// }
|
||||||
|
// return current;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // ── resetLastUpdated ─────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
// void ObservationBuffer::resetLastUpdated()
|
||||||
|
// {
|
||||||
|
// last_updated_ = robot::Time::now();
|
||||||
|
// }
|
||||||
|
|
||||||
|
// } // namespace robot_costmap_2d
|
||||||
/*********************************************************************
|
/*********************************************************************
|
||||||
*
|
*
|
||||||
* Software License Agreement (BSD License)
|
* Software License Agreement (BSD License)
|
||||||
@@ -141,7 +1044,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
|
|||||||
global_frame_, // frame đích
|
global_frame_, // frame đích
|
||||||
local_origin.header.frame_id, // frame nguồn
|
local_origin.header.frame_id, // frame nguồn
|
||||||
tf3::Time()
|
tf3::Time()
|
||||||
// data_convert::convertTime(local_origin.header.stamp)
|
// data_convert::convertTime(cloud.header.stamp)
|
||||||
);
|
);
|
||||||
tf3::doTransform(local_origin, global_origin, tfm_1);
|
tf3::doTransform(local_origin, global_origin, tfm_1);
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user