diff --git a/config/voxel_layer_params.yaml b/config/voxel_layer_params.yaml index fca4ba8..b168fdd 100644 --- a/config/voxel_layer_params.yaml +++ b/config/voxel_layer_params.yaml @@ -10,3 +10,6 @@ voxel_layer: combination_method: 1 frustum_clearing_enabled: true frustum_clearing_pixel_step: 8 + frustum_min_range: 0.20 + frustum_max_range: 3.0 + frustum_depth_camera_topic: /camera/depth/data diff --git a/include/robot_costmap_2d/observation.h b/include/robot_costmap_2d/observation.h index 49853e0..39e9b87 100755 --- a/include/robot_costmap_2d/observation.h +++ b/include/robot_costmap_2d/observation.h @@ -34,10 +34,140 @@ #include #include +#include namespace robot_costmap_2d { +/** + * @brief A depth frame and its per-source frustum-clearing configuration. + * + * The message is shared so returning buffered observations does not copy the + * full depth image on every costmap update. + */ +class DepthCameraObservation +{ +public: + DepthCameraObservation() + : data_(nullptr), + topic_(), + pixel_step_(0), + min_range_(0.0), + max_range_(0.0) + { + } + + DepthCameraObservation( + const robot_sensor_msgs::DepthCameraData& data, + std::string topic, + const robot::Time& received_time, + unsigned int pixel_step, + double min_range, + double max_range) + : data_(new robot_sensor_msgs::DepthCameraData(data)), + topic_(std::move(topic)), + received_time_(received_time), + pixel_step_(pixel_step), + min_range_(min_range), + max_range_(max_range) + { + } + + // Copy constructor: deep copy + DepthCameraObservation(const DepthCameraObservation& other) + : data_(other.data_ + ? new robot_sensor_msgs::DepthCameraData(*other.data_) + : nullptr), + topic_(other.topic_), + received_time_(other.received_time_), + pixel_step_(other.pixel_step_), + min_range_(other.min_range_), + max_range_(other.max_range_) + { + } + + // Copy assignment: deep copy + DepthCameraObservation& operator=(const DepthCameraObservation& other) + { + if (this == &other) + { + return *this; + } + + robot_sensor_msgs::DepthCameraData* new_data = nullptr; + + if (other.data_ != nullptr) + { + new_data = + new robot_sensor_msgs::DepthCameraData(*other.data_); + } + + delete data_; + data_ = new_data; + + topic_ = other.topic_; + received_time_ = other.received_time_; + pixel_step_ = other.pixel_step_; + min_range_ = other.min_range_; + max_range_ = other.max_range_; + + return *this; + } + + // Move constructor: chuyển quyền sở hữu + DepthCameraObservation(DepthCameraObservation&& other) noexcept + : data_(other.data_), + topic_(std::move(other.topic_)), + received_time_(other.received_time_), + pixel_step_(other.pixel_step_), + min_range_(other.min_range_), + max_range_(other.max_range_) + { + other.data_ = nullptr; + other.pixel_step_ = 0; + other.min_range_ = 0.0; + other.max_range_ = 0.0; + } + + // Move assignment + DepthCameraObservation& operator=(DepthCameraObservation&& other) noexcept + { + if (this == &other) + { + return *this; + } + + delete data_; + + data_ = other.data_; + topic_ = std::move(other.topic_); + received_time_ = other.received_time_; + pixel_step_ = other.pixel_step_; + min_range_ = other.min_range_; + max_range_ = other.max_range_; + + other.data_ = nullptr; + other.pixel_step_ = 0; + other.min_range_ = 0.0; + other.max_range_ = 0.0; + + return *this; + } + + ~DepthCameraObservation() + { + delete data_; + data_ = nullptr; + } + + robot_sensor_msgs::DepthCameraData* data_; + std::string topic_; + robot::Time received_time_; + unsigned int pixel_step_; + double min_range_; + double max_range_; +}; + /** * @brief Stores an observation in terms of a point cloud and the origin of the source * @note Tried to make members and constructor arguments const but the compiler would not accept the default diff --git a/include/robot_costmap_2d/observation_buffer.h b/include/robot_costmap_2d/observation_buffer.h index f348b7f..081afec 100755 --- a/include/robot_costmap_2d/observation_buffer.h +++ b/include/robot_costmap_2d/observation_buffer.h @@ -1,391 +1,3 @@ -// // /********************************************************************* -// // * -// // * 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) @@ -463,6 +75,13 @@ public: 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); + + 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, unsigned int frustum_pixel_step, double frustum_min_range, + double frustum_max_range, tf3::BufferCore& tf3_buffer, std::string global_frame, + std::string sensor_frame, double tf_tolerance); + /** * @brief Destructor... cleans up @@ -485,12 +104,22 @@ public: */ void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud); + /** + * @brief Store the newest depth frame without converting it to PointCloud2. + */ + void bufferDepthCamera(const robot_sensor_msgs::DepthCameraData& depth); + /** * @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 Append the current depth observation, if it has not expired. + */ + void getDepthObservations(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 @@ -524,6 +153,8 @@ private: */ void purgeStaleObservations(); + void purgeStaleDepthObservations(); + tf3::BufferCore& tf3_buffer_; const robot::Duration observation_keep_time_; const robot::Duration expected_update_rate_; @@ -531,11 +162,16 @@ private: std::string global_frame_; std::string sensor_frame_; std::list observation_list_; + std::list depth_observation_list_; + // DepthCameraObservation depth_observation_; 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_; + unsigned int frustum_pixel_step_; + double frustum_min_range_; + double frustum_max_range_; }; } // namespace robot_costmap_2d #endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_ diff --git a/include/robot_costmap_2d/obstacle_layer.h b/include/robot_costmap_2d/obstacle_layer.h index bcafcfb..0bc9366 100755 --- a/include/robot_costmap_2d/obstacle_layer.h +++ b/include/robot_costmap_2d/obstacle_layer.h @@ -46,6 +46,9 @@ #include +#include + +#include #include #include #include @@ -128,6 +131,12 @@ protected: void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message, const boost::shared_ptr& buffer); + /** + * @brief Buffer a depth image and its camera model for frustum clearing. + */ + void depthImageCallback(const robot_sensor_msgs::DepthCameraData& message, + const boost::shared_ptr& buffer); + /** * @brief Get the observations used to mark space * @param marking_observations A reference to a vector that will be populated with the observations @@ -142,6 +151,13 @@ protected: */ bool getClearingObservations(std::vector& clearing_observations) const; + /** + * @brief Collect fresh depth frames from every configured frustum-clearing source. + * @return True when every configured depth source is current. + */ + bool getFrustumClearingObservations( + std::vector& frustum_clearing_observations) const; + /** * @brief Clear freespace based on one observation * @param clearing_observation The observation used to raytrace @@ -170,6 +186,9 @@ protected: std::vector > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles std::vector > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles + std::vector > depth_observation_buffers_; + std::vector > depth_clearing_buffers_; + // Used only for testing purposes std::vector static_clearing_observations_, static_marking_observations_; @@ -178,6 +197,10 @@ protected: int combination_method_; std::vector callback_infos_; + std::vector callback_depth_infos_; + std::string depth_camera_data_topic_; + mutable std::mutex depth_camera_data_mutex_; + robot_sensor_msgs::DepthCameraData::ConstPtr pending_depth_camera_data_; private: bool getParams(const std::string& config_file_name, robot::NodeHandle &nh); diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index ebb1b8d..85bca7c 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -91,10 +91,11 @@ 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 raytraceDepthFrustum(double* min_x, double* min_y, double* max_x, double* max_y); + bool raytraceDepthFrustum(const robot_costmap_2d::DepthCameraObservation& observation, + double* min_x, double* min_y, double* max_x, double* max_y); + bool readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v, + double& depth_m, bool& is_valid) 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); @@ -104,8 +105,6 @@ private: 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/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index e557c4c..4fcfdf3 100755 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -131,6 +131,10 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2; std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud"; bool inf_is_valid = false, clearing=false, marking=true; + bool frustum_clearing_enabled = false; + int frustum_pixel_step = 8; + double frustum_min_range = 0.2; + double frustum_max_range = 3.0; robot::NodeHandle priv_nh(nh, source); topic = loadParam(layer[source],"topic", topic); @@ -143,6 +147,10 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa inf_is_valid = loadParam(layer[source],"inf_is_valid", false); clearing = loadParam(layer[source],"clearing", false); marking = loadParam(layer[source],"marking", true); + frustum_clearing_enabled = loadParam(layer, "frustum_clearing_enabled", false); + frustum_pixel_step = loadParam(layer, "frustum_clearing_pixel_step", 8); + frustum_min_range = loadParam(layer, "frustum_min_range", 0.2); + frustum_max_range = loadParam(layer, "frustum_max_range", 3.0); if (priv_nh.hasParam("topic")) priv_nh.getParam("topic", topic); @@ -164,52 +172,83 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa priv_nh.getParam("clearing", clearing); if (priv_nh.hasParam("marking")) priv_nh.getParam("marking", marking); - - if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")) + if (priv_nh.hasParam("frustum_clearing_enabled")) + priv_nh.getParam("frustum_clearing_enabled", frustum_clearing_enabled); + if (priv_nh.hasParam("frustum_clearing_pixel_step")) { - robot::log_error("Only topics that use point clouds or laser scans are currently supported\n"); - throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported"); + priv_nh.getParam("frustum_clearing_pixel_step", frustum_pixel_step); + frustum_pixel_step = std::max(1, frustum_pixel_step); } + if (priv_nh.hasParam("frustum_min_range")) + priv_nh.getParam("frustum_min_range", frustum_min_range); + if (priv_nh.hasParam("frustum_max_range")) + priv_nh.getParam("frustum_max_range", frustum_max_range); + if (priv_nh.hasParam("frustum_depth_camera_topic")) + priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_); - CallBackInfo info_tmp; - info_tmp.observation_source = source; - info_tmp.data_type = data_type; - info_tmp.topic = topic; - info_tmp.inf_is_valid = inf_is_valid; - callback_infos_.push_back(info_tmp); - - std::string raytrace_range_param_name, obstacle_range_param_name; + robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_pixel_step, frustum_min_range, frustum_max_range); double obstacle_range = 2.5; obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range); double raytrace_range = 3.0; raytrace_range = loadParam(layer[source],"raytrace_range", raytrace_range); - - if (priv_nh.hasParam("obstacle_range")) priv_nh.getParam("obstacle_range", obstacle_range); if (priv_nh.hasParam("raytrace_range")) priv_nh.getParam("raytrace_range", raytrace_range); - // enabled_ = enabled; + if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan" || data_type == "DepthCameraData")) + { + robot::log_error("Only topics that use point clouds or laser scans are currently supported\n"); + throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported"); + } - robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(), - priv_nh.getNamespace().c_str()); + if(!frustum_clearing_enabled) + { - // create an observation buffer - observation_buffers_.push_back( - boost::shared_ptr < ObservationBuffer - > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, - max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, - sensor_frame, transform_tolerance))); - if (marking) - marking_buffers_.push_back(observation_buffers_.back()); + CallBackInfo info_tmp; + info_tmp.observation_source = source; + info_tmp.data_type = data_type; + info_tmp.topic = topic; + info_tmp.inf_is_valid = inf_is_valid; + callback_infos_.push_back(info_tmp); - // check if we'll also add this buffer to our clearing observation buffers - if (clearing) - clearing_buffers_.push_back(observation_buffers_.back()); + // enabled_ = enabled; + robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(), + priv_nh.getNamespace().c_str()); + + // create an observation buffer + observation_buffers_.push_back( + boost::shared_ptr < ObservationBuffer + > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, + max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, + sensor_frame, transform_tolerance))); + if (marking) + marking_buffers_.push_back(observation_buffers_.back()); + + // check if we'll also add this buffer to our clearing observation buffers + if (clearing) + clearing_buffers_.push_back(observation_buffers_.back()); + } + else + { + CallBackInfo info_tmp; + info_tmp.observation_source = source; + info_tmp.data_type = data_type; + info_tmp.topic = topic; + info_tmp.inf_is_valid = inf_is_valid; + callback_depth_infos_.push_back(info_tmp); + + depth_observation_buffers_.push_back( + boost::shared_ptr < ObservationBuffer + > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, + max_obstacle_height, obstacle_range, raytrace_range, frustum_pixel_step, + frustum_min_range, frustum_max_range, *tf_, global_frame_, + sensor_frame, transform_tolerance))); + + } robot::log_info( "Created an observation buffer for topic %s, global frame: %s, " "expected update rate: %.2f, observation persistence: %.2f\n", @@ -232,7 +271,85 @@ void ObstacleLayer::handleImpl(const void* data, { if(!stop_receiving_data_) { - + if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr) ) + { + const robot_sensor_msgs::DepthCameraData::ConstPtr& depth_camera_data_ptr = + *static_cast(data); + if (!depth_camera_data_ptr) + return; + + const robot_sensor_msgs::DepthCameraData& depth_camera_data = + *depth_camera_data_ptr; + const robot_sensor_msgs::Image& depth = depth_camera_data.depth; + const robot_sensor_msgs::CameraInfo& camera_info = depth_camera_data.camera_info; + + std::size_t bytes_per_pixel = 0; + if (depth.encoding == "16UC1" || depth.encoding == "mono16") + bytes_per_pixel = 2; + else if (depth.encoding == "32FC1") + bytes_per_pixel = 4; + else + { + robot::log_error("ObstacleLayer received unsupported depth encoding: %s\n", depth.encoding.c_str()); + return; + } + + const bool invalid_dimensions = depth.width == 0 || depth.height == 0 || + depth.step < static_cast(depth.width) * bytes_per_pixel || + depth.data.size() < static_cast(depth.step) * depth.height; + if (invalid_dimensions) + { + robot::log_error("ObstacleLayer received malformed DepthCameraData image\n"); + return; + } + + if (camera_info.K[0] <= 0.0 || camera_info.K[4] <= 0.0) + { + robot::log_error("ObstacleLayer received invalid camera intrinsics for depth clearing\n"); + return; + } + + if ((camera_info.width != 0 && camera_info.width != depth.width) || + (camera_info.height != 0 && camera_info.height != depth.height)) + { + robot::log_error("ObstacleLayer received mismatched depth image and camera info dimensions\n"); + return; + } + + const std::string& depth_frame = depth.header.frame_id; + const std::string& camera_frame = camera_info.header.frame_id; + if (!depth_frame.empty() && !camera_frame.empty() && depth_frame != camera_frame) + { + robot::log_error("ObstacleLayer received mismatched depth and camera-info frames: %s != %s\n", + depth_frame.c_str(), camera_frame.c_str()); + return; + } + + if (depth_camera_data.header.frame_id.empty() && depth_frame.empty() && camera_frame.empty()) + { + robot::log_error("ObstacleLayer received DepthCameraData without an optical frame\n"); + return; + } + + // std::lock_guard lock(depth_camera_data_mutex_); + // pending_depth_camera_data_ = depth_camera_data_ptr; + if(depth_observation_buffers_.empty() || callback_depth_infos_.empty()) return; + + int size_callback_depth = static_cast(callback_depth_infos_.size()); + for(int i = 0; i < size_callback_depth; i++) + { + boost::shared_ptr& buffer = depth_observation_buffers_[i]; + if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr) && + topic == callback_depth_infos_[i].topic) + { + // robot::log_error_throttle(1.0,"TEST"); + depthImageCallback(depth_camera_data, buffer); + } + } + // return; + } + else + { if(observation_buffers_.empty() || callback_infos_.empty()) return; int size_callback = static_cast(callback_infos_.size()); @@ -286,6 +403,7 @@ void ObstacleLayer::handleImpl(const void* data, // << "topic check: " << callback_infos_[i].topic << std::endl << std::endl; // } } + } } else { @@ -392,6 +510,15 @@ void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& me buffer->unlock(); } +void ObstacleLayer::depthImageCallback(const robot_sensor_msgs::DepthCameraData& message, + const boost::shared_ptr& buffer) +{ + buffer->lock(); + // robot::log_error_throttle(1.0, "depth data size 1: %d", (int)message.depth.data.size()); + buffer->bufferDepthCamera(message); + buffer->unlock(); +} + void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) { @@ -550,6 +677,23 @@ bool ObstacleLayer::getClearingObservations(std::vector& clearing_o return current; } +bool ObstacleLayer::getFrustumClearingObservations(std::vector& frustum_clearing_observations) const +{ + bool current = true; + // DepthCameraObservation depth_obs; + + for (const boost::shared_ptr& buffer : depth_observation_buffers_) + { + buffer->lock(); + buffer->getDepthObservations(frustum_clearing_observations); + current = buffer->isCurrent() && current; + buffer->unlock(); + // frustum_clearing_observations.push_back(depth_obs); + } + + return current; +} + void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y, double* max_x, double* max_y) { diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index df37b35..b0f9821 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -37,7 +37,14 @@ *********************************************************************/ #include #include +#include +#include +#include #include +#include +#include +#include +#include #define VOXEL_BITS 16 @@ -92,7 +99,7 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl mark_threshold_ = loadParam(layer, "mark_threshold", 0); combination_method_ = loadParam(layer, "combination_method", 0.0); - int size_z, unknown_threshold, mark_threshold; + int size_z, unknown_threshold, mark_threshold, frustum_pixel_step; if (nh.hasParam("enabled")) nh.getParam("enabled", enabled_); if (nh.hasParam("footprint_clearing_enabled")) @@ -165,6 +172,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, bool current = true; std::vector observations, clearing_observations; + std::vector depth_observations; // get the marking observations current = getMarkingObservations(observations) && current; @@ -172,9 +180,16 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, // get the clearing observations current = getClearingObservations(clearing_observations) && current; + current = getFrustumClearingObservations(depth_observations) && current; + // update the global current status current_ = current; + for (const DepthCameraObservation& depth_observation : depth_observations) + { + raytraceDepthFrustum(depth_observation, min_x, min_y, max_x, max_y); + } + // raytrace freespace for (unsigned int i = 0; i < clearing_observations.size(); ++i) { @@ -231,29 +246,6 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, } } } - - // if (publish_voxel_) - // { - // robot_costmap_2d::VoxelGrid grid_msg; - // unsigned int size = robot_voxel_grid_.sizeX() * robot_voxel_grid_.sizeY(); - // grid_msg.size_x = robot_voxel_grid_.sizeX(); - // grid_msg.size_y = robot_voxel_grid_.sizeY(); - // grid_msg.size_z = robot_voxel_grid_.sizeZ(); - // grid_msg.data.resize(size); - // memcpy(&grid_msg.data[0], robot_voxel_grid_.getData(), size * sizeof(unsigned int)); - - // grid_msg.origin.x = origin_x_; - // grid_msg.origin.y = origin_y_; - // grid_msg.origin.z = origin_z_; - - // grid_msg.resolutions.x = resolution_; - // grid_msg.resolutions.y = resolution_; - // grid_msg.resolutions.z = z_resolution_; - // grid_msg.header.frame_id = global_frame_; - // grid_msg.header.stamp = robot::Time::now(); - // voxel_pub_.publish(grid_msg); - // } - updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); } @@ -327,14 +319,6 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub ox, oy, oz); return; } - - // 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); - // } - // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later double map_end_x = origin_x_ + getSizeInMetersX(); double map_end_y = origin_y_ + getSizeInMetersY(); @@ -409,26 +393,249 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub cell_raytrace_range); updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y); + } + } +} - // if (publish_clearing_points) - // { - // robot_geometry_msgs::Point32 point; - // point.x = wpx; - // point.y = wpy; - // point.z = wpz; - // clearing_endpoints_.points.push_back(point); - // } +bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v, + double& depth_m, bool& is_valid) const +{ + depth_m = 0.0; + is_valid = false; + + if (u >= depth.width || v >= depth.height) + return false; + + if (depth.encoding == "16UC1" || depth.encoding == "mono16") + { + const std::size_t offset = static_cast(v) * depth.step + static_cast(u) * 2; + if (offset + sizeof(std::uint16_t) > depth.data.size()) + return false; + + std::uint16_t raw = 0; + if (depth.is_bigendian) + raw = static_cast((depth.data[offset] << 8) | depth.data[offset + 1]); + else + raw = static_cast(depth.data[offset] | (depth.data[offset + 1] << 8)); + + if (raw == 0) + return true; + + depth_m = static_cast(raw) * 0.001; + is_valid = true; + return true; + } + + if (depth.encoding == "32FC1") + { + const std::size_t offset = static_cast(v) * depth.step + static_cast(u) * 4; + if (offset + sizeof(float) > depth.data.size()) + return false; + + float raw = 0.0f; + if (depth.is_bigendian) + { + unsigned char bytes[sizeof(float)] = { + depth.data[offset + 3], depth.data[offset + 2], depth.data[offset + 1], depth.data[offset]}; + std::memcpy(&raw, bytes, sizeof(float)); + } + else + { + std::memcpy(&raw, &depth.data[offset], sizeof(float)); + } + + if (!std::isfinite(raw) || raw <= 0.0f) + return true; + + depth_m = static_cast(raw); + is_valid = true; + return true; + } + + robot::log_error("VoxelLayer unsupported depth encoding for frustum clearing: %s\n", depth.encoding.c_str()); + return false; +} + +bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz) +{ + double a = wx - ox; + double b = wy - oy; + double c = wz - oz; + double t = 1.0; + constexpr double kEpsilon = 1e-9; + + if (std::fabs(a) < kEpsilon && std::fabs(b) < kEpsilon && std::fabs(c) < kEpsilon) + return false; + + if (wz > max_obstacle_height_ && std::fabs(c) > kEpsilon) + t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c)); + else if (wz < origin_z_ && std::fabs(c) > kEpsilon) + t = std::min(t, (origin_z_ - oz) / c); + + const double map_end_x = origin_x_ + getSizeInMetersX(); + const double map_end_y = origin_y_ + getSizeInMetersY(); + + if (wx < origin_x_ && std::fabs(a) > kEpsilon) + t = std::min(t, (origin_x_ - ox) / a); + if (wy < origin_y_ && std::fabs(b) > kEpsilon) + t = std::min(t, (origin_y_ - oy) / b); + if (wx > map_end_x && std::fabs(a) > kEpsilon) + t = std::min(t, (map_end_x - ox) / a); + if (wy > map_end_y && std::fabs(b) > kEpsilon) + t = std::min(t, (map_end_y - oy) / b); + + if (!std::isfinite(t) || t <= 0.0) + return false; + + wx = ox + a * t; + wy = oy + b * t; + wz = oz + c * t; + return true; +} + +bool VoxelLayer::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) +{ + double sensor_x, sensor_y, sensor_z; + if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) + return false; + + if (!clipRaytraceEndpoint(ox, oy, oz, wx, wy, wz)) + return false; + + double point_x, point_y, point_z; + if (!worldToMap3DFloat(wx, wy, wz, point_x, point_y, point_z)) + return false; + + robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_, + unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION, + cellDistance(raytrace_range)); + updateRaytraceBounds(ox, oy, wx, wy, raytrace_range, min_x, min_y, max_x, max_y); + return true; +} + +bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation, + double* min_x, double* min_y, double* max_x, double* max_y) +{ + if (!observation.data_) + return false; + + const robot_sensor_msgs::DepthCameraData& depth_camera_data = *observation.data_; + const robot_sensor_msgs::Image& depth = depth_camera_data.depth; + const robot_sensor_msgs::CameraInfo& camera_info = depth_camera_data.camera_info; + + if (depth.width == 0 || depth.height == 0 || depth.data.empty()) + return false; + + const double fx = camera_info.K[0]; + const double fy = camera_info.K[4]; + const double cx = camera_info.K[2]; + const double cy = camera_info.K[5]; + if (fx <= 0.0 || fy <= 0.0) + return false; + + std::string depth_frame = depth.header.frame_id.empty() ? depth_camera_data.header.frame_id : depth.header.frame_id; + if (depth_frame.empty()) + depth_frame = camera_info.header.frame_id; + if (depth_frame.empty() || tf_ == nullptr) + return false; + + robot_geometry_msgs::PointStamped local_origin; + local_origin.header = depth.header; + local_origin.header.frame_id = depth_frame; + if (local_origin.header.stamp.isZero()) + local_origin.header.stamp = depth_camera_data.header.stamp; + local_origin.point.x = 0.0; + local_origin.point.y = 0.0; + local_origin.point.z = 0.0; + + robot_geometry_msgs::PointStamped global_origin; + tf3::TransformStampedMsg tfm; + try + { + tfm = tf_->lookupTransform(global_frame_, depth_frame, tf3::Time()); + tf3::doTransform(local_origin, global_origin, tfm); + } + catch (tf3::TransformException& ex) + { + robot::log_error_throttle( + 5.0, "VoxelLayer depth topic [%s] TF exception from %s to %s: %s\n", + observation.topic_.c_str(), depth_frame.c_str(), global_frame_.c_str(), ex.what()); + return false; + } + + const double ox = global_origin.point.x; + const double oy = global_origin.point.y; + const double oz = global_origin.point.z; + + double sensor_x, sensor_y, sensor_z; + if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) + { + robot::log_error_throttle( + 5.0, "VoxelLayer depth topic [%s] origin at (%.2f, %.2f, %.2f) is outside the voxel map\n", + observation.topic_.c_str(), ox, oy, oz); + return false; + } + + const unsigned int step = std::max(1u, observation.pixel_step_); + const double min_range = observation.min_range_; + const double max_range = observation.max_range_; + const double skip_dist = 2.0 * resolution_; + const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width); + const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height); + bool cleared_any = false; + + for (unsigned int v = 0; v < height; v += step) + { + for (unsigned int u = 0; u < width; u += step) + { + double depth_m = 0.0; + bool valid = false; + if (!readDepthMeters(depth, u, v, depth_m, valid)) + continue; + + double ray_len = max_range; + if (valid && depth_m < max_range) + ray_len = std::max(0.0, depth_m - skip_dist); + + if (ray_len <= min_range) + continue; + + double dx = (static_cast(u) - cx) / fx; + double dy = (static_cast(v) - cy) / fy; + double dz = 1.0; + const double norm = std::sqrt(dx * dx + dy * dy + dz * dz); + if (norm <= 0.0) + continue; + + robot_geometry_msgs::Vector3 local_ray; + local_ray.x = dx / norm; + local_ray.y = dy / norm; + local_ray.z = dz / norm; + + robot_geometry_msgs::Vector3 global_ray; + tf3::doTransform(local_ray, global_ray, tfm); + const double global_norm = + std::sqrt(global_ray.x * global_ray.x + global_ray.y * global_ray.y + global_ray.z * global_ray.z); + if (global_norm <= 0.0) + continue; + + global_ray.x /= global_norm; + global_ray.y /= global_norm; + global_ray.z /= global_norm; + + const double sx = ox + global_ray.x * min_range; + const double sy = oy + global_ray.y * min_range; + const double sz = oz + global_ray.z * min_range; + const double wx = ox + global_ray.x * ray_len; + const double wy = oy + global_ray.y * ray_len; + const double wz = oz + global_ray.z * ray_len; + + cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, min_x, min_y, max_x, max_y) || cleared_any; } } - // 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_pub_.publish(clearing_endpoints_); - // } + return cleared_any; } void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 0d1dc15..2d8a5ef 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -385,6 +385,11 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, double max_obstacle_height; double obstacle_range; double raytrace_range; + bool frustum_clearing_enabled = false; + int frustum_clearing_pixel_step = 8; + double frustum_min_range = 0.2; + double frustum_max_range = 3.0; + 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); @@ -397,7 +402,12 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, 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); + move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_enabled", frustum_clearing_enabled); + move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_pixel_step", frustum_clearing_pixel_step); + move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_min_range", frustum_min_range); + move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_max_range", frustum_max_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("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range); } } } @@ -434,6 +444,11 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, double max_obstacle_height; double obstacle_range; double raytrace_range; + bool frustum_clearing_enabled = false; + int frustum_clearing_pixel_step = 8; + double frustum_min_range = 0.2; + double frustum_max_range = 3.0; + 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); @@ -446,7 +461,12 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, 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); + move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_enabled", frustum_clearing_enabled); + move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_pixel_step", frustum_clearing_pixel_step); + move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_min_range", frustum_min_range); + move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_max_range", frustum_max_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("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range); } } } diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index 224c559..ae69447 100755 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -1,906 +1,3 @@ -// // /********************************************************************* -// // * -// // * 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) @@ -959,6 +56,23 @@ ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_ { } +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, unsigned int frustum_pixel_step, + double frustum_min_range, double frustum_max_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), + frustum_pixel_step_(std::max(1u, frustum_pixel_step)), + frustum_min_range_(std::max(0.0, frustum_min_range)), + frustum_max_range_(std::max(frustum_max_range, frustum_min_range_)), + tf_tolerance_(tf_tolerance) +{ +} + ObservationBuffer::~ObservationBuffer() { } @@ -1122,6 +236,30 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud) purgeStaleObservations(); } +void ObservationBuffer::bufferDepthCamera(const robot_sensor_msgs::DepthCameraData& depth_camera_data) +{ + depth_observation_list_.push_front(DepthCameraObservation()); + if (depth_observation_list_.front().data_ == nullptr) + { + depth_observation_list_.front().data_ = + new robot_sensor_msgs::DepthCameraData(depth_camera_data); + } + else + { + *depth_observation_list_.front().data_ = depth_camera_data; + } + + depth_observation_list_.front().pixel_step_ = frustum_pixel_step_; + depth_observation_list_.front().min_range_ = frustum_min_range_; + depth_observation_list_.front().max_range_ = frustum_max_range_; + + // if the update was successful, we want to update the last updated time + last_updated_ = robot::Time::now(); + + // first... let's make sure that we don't have any stale observations + purgeStaleDepthObservations(); +} + // returns a copy of the observations void ObservationBuffer::getObservations(vector& observations) { @@ -1136,6 +274,19 @@ void ObservationBuffer::getObservations(vector& observations) } } +void ObservationBuffer::getDepthObservations(vector& observations) +{ + // first... let's make sure that we don't have any stale observations + purgeStaleDepthObservations(); + + // now we'll just copy the observations for the caller + list::iterator obs_it; + for (obs_it = depth_observation_list_.begin(); obs_it != depth_observation_list_.end(); ++obs_it) + { + observations.push_back(*obs_it); + } +} + void ObservationBuffer::purgeStaleObservations() { if (!observation_list_.empty()) @@ -1162,6 +313,30 @@ void ObservationBuffer::purgeStaleObservations() } } +void ObservationBuffer::purgeStaleDepthObservations() +{ + if (depth_observation_list_.empty()) + return; + + if (observation_keep_time_ == robot::Duration(0.0)) + { + auto observation = depth_observation_list_.begin(); + depth_observation_list_.erase(++observation, depth_observation_list_.end()); + return; + } + + const robot::Time now = robot::Time::now(); + for (auto observation = depth_observation_list_.begin(); observation != depth_observation_list_.end(); ++observation) + { + DepthCameraObservation& obs = *observation; + if ((last_updated_ - obs.data_->header.stamp) > observation_keep_time_) + { + depth_observation_list_.erase(observation, depth_observation_list_.end()); + return; + } + } +} + bool ObservationBuffer::isCurrent() const { if (expected_update_rate_ == robot::Duration(0.0))