From a2a021c114eece51eb6ada705d90d0dabd3e10f4 Mon Sep 17 00:00:00 2001 From: duongtd Date: Fri, 10 Jul 2026 11:27:20 +0700 Subject: [PATCH] add function computeCost file layer.h --- config/voxel_layer_params.yaml | 3 +- include/robot_costmap_2d/inflation_layer.h | 3 +- include/robot_costmap_2d/layer.h | 2 + include/robot_costmap_2d/observation_buffer.h | 388 ++++++++ include/robot_costmap_2d/voxel_layer.h | 9 + plugins/directional_layer.cpp | 8 - plugins/static_layer.cpp | 23 +- plugins/voxel_layer.cpp | 20 +- src/costmap_2d_robot.cpp | 20 + src/observation_buffer.cpp | 905 +++++++++++++++++- 10 files changed, 1355 insertions(+), 26 deletions(-) diff --git a/config/voxel_layer_params.yaml b/config/voxel_layer_params.yaml index 3dc96dd..fca4ba8 100644 --- a/config/voxel_layer_params.yaml +++ b/config/voxel_layer_params.yaml @@ -8,4 +8,5 @@ voxel_layer: unknown_threshold: 15.0 mark_threshold: 0 combination_method: 1 - + frustum_clearing_enabled: true + frustum_clearing_pixel_step: 8 diff --git a/include/robot_costmap_2d/inflation_layer.h b/include/robot_costmap_2d/inflation_layer.h index a8fb5c2..3b4c855 100755 --- a/include/robot_costmap_2d/inflation_layer.h +++ b/include/robot_costmap_2d/inflation_layer.h @@ -96,8 +96,9 @@ public: /** @brief Given a distance, compute a cost. * @param distance The distance from an obstacle in cells * @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; if (distance == 0) cost = LETHAL_OBSTACLE; diff --git a/include/robot_costmap_2d/layer.h b/include/robot_costmap_2d/layer.h index 36da261..066aded 100755 --- a/include/robot_costmap_2d/layer.h +++ b/include/robot_costmap_2d/layer.h @@ -85,6 +85,8 @@ public: */ 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. */ virtual void deactivate() {} diff --git a/include/robot_costmap_2d/observation_buffer.h b/include/robot_costmap_2d/observation_buffer.h index 2d1d482..f348b7f 100755 --- a/include/robot_costmap_2d/observation_buffer.h +++ b/include/robot_costmap_2d/observation_buffer.h @@ -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 +// // #include +// // #include +// // #include +// // #include +// // #include +// // #include + +// // // Thread support +// // #include + +// // 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 +// // * Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier +// // * @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& 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_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 +// #include +// #include +// #include +// #include +// #include + +// #include +// #include +// #include +// #include + +// // Thread support +// #include + +// 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& 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_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) diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index da08c78..ebb1b8d 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -91,12 +91,21 @@ private: 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, 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_; robot_voxel_grid::VoxelGrid robot_voxel_grid_; double z_resolution_, origin_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_; inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz) diff --git a/plugins/directional_layer.cpp b/plugins/directional_layer.cpp index 260af1f..ae8e3fa 100644 --- a/plugins/directional_layer.cpp +++ b/plugins/directional_layer.cpp @@ -162,14 +162,6 @@ namespace robot_costmap_2d robot_nav_msgs::OccupancyGrid lanes; convertToMap(costmap_, lanes, 0.65, 0.196); - ////////////////////////////////// - ////////////////////////////////// - /////////THAY THẾ PUBLISH//////// - // lane_mask_pub_.publish(lanes); - ////////////////////////////////// - ////////////////////////////////// - ////////////////////////////////// - return false; } diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index 2add362..6ca4bf7 100755 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -44,6 +44,7 @@ #include #include +#include using robot_costmap_2d::NO_INFORMATION; @@ -71,6 +72,7 @@ void StaticLayer::onInitialize() global_frame_ = layered_costmap_->getGlobalFrameID(); std::string config_file_name = "static_layer_params.yaml"; 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) @@ -191,12 +193,23 @@ void StaticLayer::handleImpl(const void* data, const std::type_info& type, 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(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(data)); } 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_) { - 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; 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; try { - transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time::now()); + transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time()); } catch (tf3::TransformException ex) { diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index 9f94e7a..df37b35 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -331,8 +331,8 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub // bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0); // if (publish_clearing_points) // { - clearing_endpoints_.points.clear(); - clearing_endpoints_.points.reserve(clearing_observation_cloud_size); + // clearing_endpoints_.points.clear(); + // 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 @@ -412,20 +412,20 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub // if (publish_clearing_points) // { - robot_geometry_msgs::Point32 point; - point.x = wpx; - point.y = wpy; - point.z = wpz; - clearing_endpoints_.points.push_back(point); + // robot_geometry_msgs::Point32 point; + // point.x = wpx; + // point.y = wpy; + // point.z = wpz; + // clearing_endpoints_.points.push_back(point); // } } } // if (publish_clearing_points) // { - clearing_endpoints_.header.frame_id = global_frame_; - clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp; - clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq; + // clearing_endpoints_.header.frame_id = global_frame_; + // clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp; + // clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq; // clearing_endpoints_pub_.publish(clearing_endpoints_); // } diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 626047a..0d1dc15 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -378,15 +378,25 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, bool clearing; bool marking; bool inf_is_valid; + std::string sensor_frame; + double observation_persistence; + double expected_update_rate; double min_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, "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, "clearing", clearing); 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, "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, "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); } } @@ -417,15 +427,25 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, bool clearing; bool marking; bool inf_is_valid; + std::string sensor_frame; + double observation_persistence; + double expected_update_rate; double min_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, "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, "clearing", clearing); 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, "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, "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); } } diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index fdc1869..224c559 100755 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -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 + +// // #include +// // #include +// // #include + +// // 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::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 iter_z(global_frame_cloud, "z"); +// // // std::vector::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end(); +// // // std::vector::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(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(gx); +// // // const float gyf = static_cast(gy); +// // // const float gzf = static_cast(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 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 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(std::floor(gx * inv_voxel_size)); +// // const int32_t iy = static_cast(std::floor(gy * inv_voxel_size)); + +// // // Pack 2×int32 thành 1×int64 làm key +// // const int64_t key = (static_cast(ix) << 32) | +// // static_cast(static_cast(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(gx); +// // vd.y = static_cast(gy); +// // vd.z = static_cast(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(voxel_map.size()); +// // obs_cloud.data.resize(static_cast(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& 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::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::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 + +// #include +// #include +// #include + +// #include +// #include +// #include + +// 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(f.offset); +// else if (f.name == "y") y_off = static_cast(f.offset); +// else if (f.name == "z") z_off = static_cast(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 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(cloud_size) * point_step); + +// // Voxel map: key → byte offset in obs_cloud.data (first-hit written directly) +// std::unordered_map 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(std::floor(gx * inv_vs)); +// const int32_t iy = static_cast(std::floor(gy * inv_vs)); + +// // Pack into single int64 key +// const int64_t key = +// (static_cast(ix) << 32) | +// static_cast(static_cast(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(gx); +// const float gyf = static_cast(gy); +// const float gzf = static_cast(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& 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) @@ -141,7 +1044,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud) global_frame_, // frame đích local_origin.header.frame_id, // frame nguồn tf3::Time() - // data_convert::convertTime(local_origin.header.stamp) + // data_convert::convertTime(cloud.header.stamp) ); tf3::doTransform(local_origin, global_origin, tfm_1);