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

add multi camera depth

Browse Source
This commit is contained in:
duongtd
2026-07-14 09:42:35 +07:00
parent a2a021c114
commit 6a9834d3a8
9 changed files with 711 additions and 1374 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
config/voxel_layer_params.yaml
View File

@@ -10,3 +10,6 @@ voxel_layer:
combination_method: 1 combination_method: 1
frustum_clearing_enabled: true frustum_clearing_enabled: true
frustum_clearing_pixel_step: 8 frustum_clearing_pixel_step: 8
frustum_min_range: 0.20
frustum_max_range: 3.0
frustum_depth_camera_topic: /camera/depth/data

130
include/robot_costmap_2d/observation.h
View File

@@ -34,10 +34,140 @@
#include <robot_geometry_msgs/Point.h> #include <robot_geometry_msgs/Point.h>
#include <robot_sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/DepthCameraData.h>
namespace robot_costmap_2d 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 * @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 * @note Tried to make members and constructor arguments const but the compiler would not accept the default

412
include/robot_costmap_2d/observation_buffer.h
View File

@@ -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 <vector>
// // #include <list>
// // #include <string>
// // #include <robot/robot.h>
// // #include <robot_costmap_2d/observation.h>
// // #include <tf3/buffer_core.h>
// // #include <robot_sensor_msgs/PointCloud2.h>
// // // Thread support
// // #include <boost/thread.hpp>
// // namespace robot_costmap_2d
// // {
// // /**
// // * @class ObservationBuffer
// // * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them
// // */
// // class ObservationBuffer
// // {
// // public:
// // /**
// // * @brief Constructs an observation buffer
// // * @param topic_name The topic of the observations, used as an identifier for error and warning messages
// // * @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
// // * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
// // * @param min_obstacle_height The minimum height of a hitpoint to be considered legal
// // * @param max_obstacle_height The minimum height of a hitpoint to be considered legal
// // * @param obstacle_range The range to which the sensor should be trusted for inserting obstacles
// // * @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
// // * @param tf2_buffer A reference to a tf2 Buffer
// // * @param global_frame The frame to transform PointClouds into
// // * @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
// // * @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
// // */
// // ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
// // double min_obstacle_height, double max_obstacle_height, double obstacle_range,
// // double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
// // std::string sensor_frame, double tf_tolerance);
// // /**
// // * @brief Destructor... cleans up
// // */
// // ~ObservationBuffer();
// // /**
// // * @brief Sets the global frame of an observation buffer. This will
// // * transform all the currently cached observations to the new global
// // * frame
// // * @param new_global_frame The name of the new global frame.
// // * @return True if the operation succeeds, false otherwise
// // */
// // bool setGlobalFrame(const std::string new_global_frame);
// // /**
// // * @brief Transforms a PointCloud to the global frame and buffers it
// // * <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
// // * @param cloud The cloud to be buffered
// // */
// // void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
// // /**
// // * @brief Pushes copies of all current observations onto the end of the vector passed in
// // * @param observations The vector to be filled
// // */
// // void getObservations(std::vector<Observation>& observations);
// // /**
// // * @brief Check if the observation buffer is being update at its expected rate
// // * @return True if it is being updated at the expected rate, false otherwise
// // */
// // bool isCurrent() const;
// // /**
// // * @brief Lock the observation buffer
// // */
// // inline void lock()
// // {
// // lock_.lock();
// // }
// // /**
// // * @brief Lock the observation buffer
// // */
// // inline void unlock()
// // {
// // lock_.unlock();
// // }
// // /**
// // * @brief Reset last updated timestamp
// // */
// // void resetLastUpdated();
// // private:
// // /**
// // * @brief Removes any stale observations from the buffer list
// // */
// // void purgeStaleObservations();
// // // Helper: trích 4×4 transform matrix từ TransformStampedMsg
// // // Tránh gọi tf3::doTransform per-point (overhead virtual dispatch + exception check)
// // struct Transform4x4 {
// // double m[4][4];
// // };
// // static inline Transform4x4 extractMatrix(const tf3::TransformStampedMsg& tfm)
// // {
// // // Quaternion → rotation matrix + translation
// // const auto& t = tfm.transform.translation;
// // const auto& q = tfm.transform.rotation;
// // double qx = q.x, qy = q.y, qz = q.z, qw = q.w;
// // Transform4x4 M;
// // M.m[0][0] = 1 - 2*(qy*qy + qz*qz); M.m[0][1] = 2*(qx*qy - qz*qw); M.m[0][2] = 2*(qx*qz + qy*qw); M.m[0][3] = t.x;
// // M.m[1][0] = 2*(qx*qy + qz*qw); M.m[1][1] = 1 - 2*(qx*qx + qz*qz); M.m[1][2] = 2*(qy*qz - qx*qw); M.m[1][3] = t.y;
// // M.m[2][0] = 2*(qx*qz - qy*qw); M.m[2][1] = 2*(qy*qz + qx*qw); M.m[2][2] = 1 - 2*(qx*qx + qy*qy); M.m[2][3] = t.z;
// // M.m[3][0] = 0; M.m[3][1] = 0; M.m[3][2] = 0; M.m[3][3] = 1;
// // return M;
// // }
// // double voxel_size_;
// // tf3::BufferCore& tf3_buffer_;
// // const robot::Duration observation_keep_time_;
// // const robot::Duration expected_update_rate_;
// // robot::Time last_updated_;
// // std::string global_frame_;
// // std::string sensor_frame_;
// // std::list<Observation> observation_list_;
// // std::string topic_name_;
// // double min_obstacle_height_, max_obstacle_height_;
// // boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
// // double obstacle_range_, raytrace_range_;
// // double tf_tolerance_;
// // };
// // } // namespace robot_costmap_2d
// // #endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
// /*********************************************************************
// *
// * Software License Agreement (BSD License)
// *
// * Copyright (c) 2008, 2013, Willow Garage, Inc.
// * All rights reserved.
// *
// * Redistribution and use in source and binary forms, with or without
// * modification, are permitted provided that the following conditions
// * are met:
// *
// * * Redistributions of source code must retain the above copyright
// * notice, this list of conditions and the following disclaimer.
// * * Redistributions in binary form must reproduce the above
// * copyright notice, this list of conditions and the following
// * disclaimer in the documentation and/or other materials provided
// * with the distribution.
// * * Neither the name of Willow Garage, Inc. nor the names of its
// * contributors may be used to endorse or promote products derived
// * from this software without specific prior written permission.
// *
// * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
// * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
// * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// * POSSIBILITY OF SUCH DAMAGE.
// *
// * Author: Eitan Marder-Eppstein
// *********************************************************************/
// #ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
// #define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
// #include <vector>
// #include <list>
// #include <string>
// #include <unordered_map>
// #include <cmath>
// #include <cstring>
// #include <robot/robot.h>
// #include <robot_costmap_2d/observation.h>
// #include <tf3/buffer_core.h>
// #include <robot_sensor_msgs/PointCloud2.h>
// // Thread support
// #include <boost/thread.hpp>
// namespace robot_costmap_2d
// {
// /**
// * @class ObservationBuffer
// * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them.
// *
// * Optimizations vs original:
// * - bufferCloud: single-pass transform+filter+voxel-downsample.
// * Reduces 6.5 M points to at most (map_w × map_h) representative points,
// * which cuts CPU in updateBounds/raytraceFreespace by ~100–200×.
// * - extractMatrix: inline quaternion→rotation, avoids per-point virtual dispatch.
// * - voxel_size_ (default = costmap resolution, 0.05 m): configurable via
// * setVoxelSize() so ObstacleLayer can pass the real resolution.
// */
// class ObservationBuffer
// {
// public:
// /**
// * @brief Constructs an observation buffer
// * @param topic_name The topic of the observations, used as an identifier for error and warning messages
// * @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
// * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
// * @param min_obstacle_height The minimum height of a hitpoint to be considered legal
// * @param max_obstacle_height The maximum height of a hitpoint to be considered legal
// * @param obstacle_range The range to which the sensor should be trusted for inserting obstacles
// * @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
// * @param tf2_buffer A reference to a tf2 Buffer
// * @param global_frame The frame to transform PointClouds into
// * @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
// * @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
// */
// ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
// double min_obstacle_height, double max_obstacle_height, double obstacle_range,
// double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
// std::string sensor_frame, double tf_tolerance);
// /**
// * @brief Destructor... cleans up
// */
// ~ObservationBuffer();
// /**
// * @brief Sets the global frame of an observation buffer. This will
// * transform all the currently cached observations to the new global frame
// * @param new_global_frame The name of the new global frame.
// * @return True if the operation succeeds, false otherwise
// */
// bool setGlobalFrame(const std::string new_global_frame);
// /**
// * @brief Set the voxel size used for downsampling in bufferCloud().
// * Should match the costmap resolution (default 0.05 m).
// */
// inline void setVoxelSize(double voxel_size)
// {
// voxel_size_ = voxel_size;
// inv_voxel_size_ = (voxel_size > 1e-9) ? 1.0 / voxel_size : 20.0;
// }
// /**
// * @brief Transforms a PointCloud to the global frame, downsamples it via
// * voxel grid (one representative point per costmap cell), applies
// * height filtering, and buffers the result.
// */
// void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
// /**
// * @brief Pushes copies of all current observations onto the end of the vector passed in
// * @param observations The vector to be filled
// */
// void getObservations(std::vector<Observation>& observations);
// /**
// * @brief Check if the observation buffer is being updated at its expected rate
// * @return True if it is being updated at the expected rate, false otherwise
// */
// bool isCurrent() const;
// /**
// * @brief Lock the observation buffer
// */
// inline void lock() { lock_.lock(); }
// /**
// * @brief Unlock the observation buffer
// */
// inline void unlock() { lock_.unlock(); }
// /**
// * @brief Reset last updated timestamp
// */
// void resetLastUpdated();
// private:
// /**
// * @brief Removes any stale observations from the buffer list
// */
// void purgeStaleObservations();
// // ── Transform helper ────────────────────────────────────────────────────
// // Encode a TF transform as a plain 4×4 double matrix so the hot loop in
// // bufferCloud can do a simple FMA multiply without any virtual dispatch,
// // exception handling, or iterator overhead.
// struct Transform4x4
// {
// double m[4][4];
// };
// static inline Transform4x4 extractMatrix(const tf3::TransformStampedMsg& tfm)
// {
// const auto& t = tfm.transform.translation;
// const auto& q = tfm.transform.rotation;
// const double qx = q.x, qy = q.y, qz = q.z, qw = q.w;
// Transform4x4 M;
// // Row 0
// M.m[0][0] = 1.0 - 2.0*(qy*qy + qz*qz);
// M.m[0][1] = 2.0*(qx*qy - qz*qw);
// M.m[0][2] = 2.0*(qx*qz + qy*qw);
// M.m[0][3] = t.x;
// // Row 1
// M.m[1][0] = 2.0*(qx*qy + qz*qw);
// M.m[1][1] = 1.0 - 2.0*(qx*qx + qz*qz);
// M.m[1][2] = 2.0*(qy*qz - qx*qw);
// M.m[1][3] = t.y;
// // Row 2
// M.m[2][0] = 2.0*(qx*qz - qy*qw);
// M.m[2][1] = 2.0*(qy*qz + qx*qw);
// M.m[2][2] = 1.0 - 2.0*(qx*qx + qy*qy);
// M.m[2][3] = t.z;
// // Row 3 (homogeneous)
// M.m[3][0] = 0.0; M.m[3][1] = 0.0; M.m[3][2] = 0.0; M.m[3][3] = 1.0;
// return M;
// }
// // ── Data members ────────────────────────────────────────────────────────
// tf3::BufferCore& tf3_buffer_;
// const robot::Duration observation_keep_time_;
// const robot::Duration expected_update_rate_;
// robot::Time last_updated_;
// std::string global_frame_;
// std::string sensor_frame_;
// std::list<Observation> observation_list_;
// std::string topic_name_;
// double min_obstacle_height_;
// double max_obstacle_height_;
// boost::recursive_mutex lock_;
// double obstacle_range_;
// double raytrace_range_;
// double tf_tolerance_;
// // Voxel-grid downsampling parameters (set via setVoxelSize)
// double voxel_size_ = 0.05; // metres – match costmap resolution
// double inv_voxel_size_ = 20.0; // 1/voxel_size_, cached
// };
// } // namespace robot_costmap_2d
// #endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
/********************************************************************* /*********************************************************************
* *
* Software License Agreement (BSD License) * Software License Agreement (BSD License)
@@ -464,6 +76,13 @@ public:
double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame, double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
std::string sensor_frame, double tf_tolerance); 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 * @brief Destructor... cleans up
*/ */
@@ -485,12 +104,22 @@ public:
*/ */
void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud); 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 * @brief Pushes copies of all current observations onto the end of the vector passed in
* @param observations The vector to be filled * @param observations The vector to be filled
*/ */
void getObservations(std::vector<Observation>& observations); void getObservations(std::vector<Observation>& observations);
/**
* @brief Append the current depth observation, if it has not expired.
*/
void getDepthObservations(std::vector<DepthCameraObservation>& observations);
/** /**
* @brief Check if the observation buffer is being update at its expected rate * @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 * @return True if it is being updated at the expected rate, false otherwise
@@ -524,6 +153,8 @@ private:
*/ */
void purgeStaleObservations(); void purgeStaleObservations();
void purgeStaleDepthObservations();
tf3::BufferCore& tf3_buffer_; tf3::BufferCore& tf3_buffer_;
const robot::Duration observation_keep_time_; const robot::Duration observation_keep_time_;
const robot::Duration expected_update_rate_; const robot::Duration expected_update_rate_;
@@ -531,11 +162,16 @@ private:
std::string global_frame_; std::string global_frame_;
std::string sensor_frame_; std::string sensor_frame_;
std::list<Observation> observation_list_; std::list<Observation> observation_list_;
std::list<DepthCameraObservation> depth_observation_list_;
// DepthCameraObservation depth_observation_;
std::string topic_name_; std::string topic_name_;
double min_obstacle_height_, max_obstacle_height_; double min_obstacle_height_, max_obstacle_height_;
boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
double obstacle_range_, raytrace_range_; double obstacle_range_, raytrace_range_;
double tf_tolerance_; double tf_tolerance_;
unsigned int frustum_pixel_step_;
double frustum_min_range_;
double frustum_max_range_;
}; };
} // namespace robot_costmap_2d } // namespace robot_costmap_2d
#endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_ #endif // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_

23
include/robot_costmap_2d/obstacle_layer.h
View File

@@ -46,6 +46,9 @@
#include <robot_nav_msgs/OccupancyGrid.h> #include <robot_nav_msgs/OccupancyGrid.h>
#include <mutex>
#include <robot_sensor_msgs/DepthCameraData.h>
#include <robot_sensor_msgs/LaserScan.h> #include <robot_sensor_msgs/LaserScan.h>
#include <robot_laser_geometry/laser_geometry.hpp> #include <robot_laser_geometry/laser_geometry.hpp>
#include <robot_sensor_msgs/PointCloud.h> #include <robot_sensor_msgs/PointCloud.h>
@@ -128,6 +131,12 @@ protected:
void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message, void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer); const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& 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<robot_costmap_2d::ObservationBuffer>& buffer);
/** /**
* @brief Get the observations used to mark space * @brief Get the observations used to mark space
* @param marking_observations A reference to a vector that will be populated with the observations * @param marking_observations A reference to a vector that will be populated with the observations
@@ -142,6 +151,13 @@ protected:
*/ */
bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& clearing_observations) const; bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& 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<robot_costmap_2d::DepthCameraObservation>& frustum_clearing_observations) const;
/** /**
* @brief Clear freespace based on one observation * @brief Clear freespace based on one observation
* @param clearing_observation The observation used to raytrace * @param clearing_observation The observation used to raytrace
@@ -170,6 +186,9 @@ protected:
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > depth_observation_buffers_;
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > depth_clearing_buffers_;
// Used only for testing purposes // Used only for testing purposes
std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_; std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
@@ -178,6 +197,10 @@ protected:
int combination_method_; int combination_method_;
std::vector<CallBackInfo> callback_infos_; std::vector<CallBackInfo> callback_infos_;
std::vector<CallBackInfo> 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: private:
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh); bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);

11
include/robot_costmap_2d/voxel_layer.h
View File

@@ -91,10 +91,11 @@ private:
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info); void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y, virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y); double* max_x, double* max_y);
bool raytraceDepthFrustum(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y, // bool raytraceDepthFrustum(double* min_x, double* min_y, double* max_x, double* max_y);
double* max_x, double* max_y); bool raytraceDepthFrustum(const robot_costmap_2d::DepthCameraObservation& observation,
bool getCloudPoint(const robot_sensor_msgs::PointCloud2& cloud, unsigned int u, unsigned int v, double* min_x, double* min_y, double* max_x, double* max_y);
double& wx, double& wy, double& wz) const; 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 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, 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); 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_; robot_voxel_grid::VoxelGrid robot_voxel_grid_;
double z_resolution_, origin_z_; double z_resolution_, origin_z_;
unsigned int unknown_threshold_, mark_threshold_, size_z_; unsigned int unknown_threshold_, mark_threshold_, size_z_;
bool frustum_clearing_enabled_;
unsigned int frustum_clearing_pixel_step_;
robot_sensor_msgs::PointCloud clearing_endpoints_; robot_sensor_msgs::PointCloud clearing_endpoints_;
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz) inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)

174
plugins/obstacle_layer.cpp
View File

@@ -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; 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"; std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud";
bool inf_is_valid = false, clearing=false, marking=true; 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); robot::NodeHandle priv_nh(nh, source);
topic = loadParam(layer[source],"topic", topic); 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); inf_is_valid = loadParam(layer[source],"inf_is_valid", false);
clearing = loadParam(layer[source],"clearing", false); clearing = loadParam(layer[source],"clearing", false);
marking = loadParam(layer[source],"marking", true); 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")) if (priv_nh.hasParam("topic"))
priv_nh.getParam("topic", topic); priv_nh.getParam("topic", topic);
@@ -164,13 +172,41 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
priv_nh.getParam("clearing", clearing); priv_nh.getParam("clearing", clearing);
if (priv_nh.hasParam("marking")) if (priv_nh.hasParam("marking"))
priv_nh.getParam("marking", marking); priv_nh.getParam("marking", marking);
if (priv_nh.hasParam("frustum_clearing_enabled"))
priv_nh.getParam("frustum_clearing_enabled", frustum_clearing_enabled);
if (priv_nh.hasParam("frustum_clearing_pixel_step"))
{
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_);
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")) 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);
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"); 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"); throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
} }
if(!frustum_clearing_enabled)
{
CallBackInfo info_tmp; CallBackInfo info_tmp;
info_tmp.observation_source = source; info_tmp.observation_source = source;
info_tmp.data_type = data_type; info_tmp.data_type = data_type;
@@ -178,20 +214,6 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
info_tmp.inf_is_valid = inf_is_valid; info_tmp.inf_is_valid = inf_is_valid;
callback_infos_.push_back(info_tmp); callback_infos_.push_back(info_tmp);
std::string raytrace_range_param_name, obstacle_range_param_name;
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; // enabled_ = enabled;
robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(), robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
@@ -209,7 +231,24 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
// check if we'll also add this buffer to our clearing observation buffers // check if we'll also add this buffer to our clearing observation buffers
if (clearing) if (clearing)
clearing_buffers_.push_back(observation_buffers_.back()); 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( robot::log_info(
"Created an observation buffer for topic %s, global frame: %s, " "Created an observation buffer for topic %s, global frame: %s, "
"expected update rate: %.2f, observation persistence: %.2f\n", "expected update rate: %.2f, observation persistence: %.2f\n",
@@ -232,7 +271,85 @@ void ObstacleLayer::handleImpl(const void* data,
{ {
if(!stop_receiving_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<const robot_sensor_msgs::DepthCameraData::ConstPtr*>(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<std::size_t>(depth.width) * bytes_per_pixel ||
depth.data.size() < static_cast<std::size_t>(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<std::mutex> 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<int>(callback_depth_infos_.size());
for(int i = 0; i < size_callback_depth; i++)
{
boost::shared_ptr<ObservationBuffer>& 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; if(observation_buffers_.empty() || callback_infos_.empty()) return;
int size_callback = static_cast<int>(callback_infos_.size()); int size_callback = static_cast<int>(callback_infos_.size());
@@ -287,6 +404,7 @@ void ObstacleLayer::handleImpl(const void* data,
// } // }
} }
} }
}
else else
{ {
robot::log_info("Stop receiving data!\n"); robot::log_info("Stop receiving data!\n");
@@ -392,6 +510,15 @@ void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& me
buffer->unlock(); buffer->unlock();
} }
void ObstacleLayer::depthImageCallback(const robot_sensor_msgs::DepthCameraData& message,
const boost::shared_ptr<ObservationBuffer>& 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, void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y) double* min_y, double* max_x, double* max_y)
{ {
@@ -550,6 +677,23 @@ bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_o
return current; return current;
} }
bool ObstacleLayer::getFrustumClearingObservations(std::vector<DepthCameraObservation>& frustum_clearing_observations) const
{
bool current = true;
// DepthCameraObservation depth_obs;
for (const boost::shared_ptr<ObservationBuffer>& 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, void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y) double* max_x, double* max_y)
{ {

303
plugins/voxel_layer.cpp
View File

@@ -37,7 +37,14 @@
*********************************************************************/ *********************************************************************/
#include <robot_costmap_2d/voxel_layer.h> #include <robot_costmap_2d/voxel_layer.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h> #include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_geometry_msgs/Vector3.h>
#include <tf3/exceptions.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <cstring>
#define VOXEL_BITS 16 #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); mark_threshold_ = loadParam(layer, "mark_threshold", 0);
combination_method_ = loadParam(layer, "combination_method", 0.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")) if (nh.hasParam("enabled"))
nh.getParam("enabled", enabled_); nh.getParam("enabled", enabled_);
if (nh.hasParam("footprint_clearing_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; bool current = true;
std::vector<Observation> observations, clearing_observations; std::vector<Observation> observations, clearing_observations;
std::vector<DepthCameraObservation> depth_observations;
// get the marking observations // get the marking observations
current = getMarkingObservations(observations) && current; current = getMarkingObservations(observations) && current;
@@ -172,9 +180,16 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
// get the clearing observations // get the clearing observations
current = getClearingObservations(clearing_observations) && current; current = getClearingObservations(clearing_observations) && current;
current = getFrustumClearingObservations(depth_observations) && current;
// update the global current status // update the global current status
current_ = current; current_ = current;
for (const DepthCameraObservation& depth_observation : depth_observations)
{
raytraceDepthFrustum(depth_observation, min_x, min_y, max_x, max_y);
}
// raytrace freespace // raytrace freespace
for (unsigned int i = 0; i < clearing_observations.size(); ++i) 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); 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); ox, oy, oz);
return; 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 // 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_x = origin_x_ + getSizeInMetersX();
double map_end_y = origin_y_ + getSizeInMetersY(); double map_end_y = origin_y_ + getSizeInMetersY();
@@ -409,26 +393,249 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
cell_raytrace_range); cell_raytrace_range);
updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y); updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
}
}
}
// if (publish_clearing_points) bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
// { double& depth_m, bool& is_valid) const
// robot_geometry_msgs::Point32 point; {
// point.x = wpx; depth_m = 0.0;
// point.y = wpy; is_valid = false;
// point.z = wpz;
// clearing_endpoints_.points.push_back(point); if (u >= depth.width || v >= depth.height)
// } return false;
if (depth.encoding == "16UC1" || depth.encoding == "mono16")
{
const std::size_t offset = static_cast<std::size_t>(v) * depth.step + static_cast<std::size_t>(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<std::uint16_t>((depth.data[offset] << 8) | depth.data[offset + 1]);
else
raw = static_cast<std::uint16_t>(depth.data[offset] | (depth.data[offset + 1] << 8));
if (raw == 0)
return true;
depth_m = static_cast<double>(raw) * 0.001;
is_valid = true;
return true;
}
if (depth.encoding == "32FC1")
{
const std::size_t offset = static_cast<std::size_t>(v) * depth.step + static_cast<std::size_t>(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<double>(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<double>(u) - cx) / fx;
double dy = (static_cast<double>(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) return cleared_any;
// {
// 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_);
// }
} }
void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)

20
src/costmap_2d_robot.cpp
View File

@@ -385,6 +385,11 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
double max_obstacle_height; double max_obstacle_height;
double obstacle_range; double obstacle_range;
double raytrace_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, "topic", topic);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "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, "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, "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, "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("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 max_obstacle_height;
double obstacle_range; double obstacle_range;
double raytrace_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, "topic", topic);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence); move_parameter(plugin_nh_element, costmap_plugin_nh_element, "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, "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, "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, "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("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);
} }
} }
} }

981
src/observation_buffer.cpp
View File

File diff suppressed because it is too large Load Diff