add multi camera depth
This commit is contained in:
@@ -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
|
||||
|
||||
@@ -34,10 +34,140 @@
|
||||
|
||||
#include <robot_geometry_msgs/Point.h>
|
||||
#include <robot_sensor_msgs/PointCloud2.h>
|
||||
#include <robot_sensor_msgs/DepthCameraData.h>
|
||||
|
||||
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
|
||||
|
||||
@@ -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)
|
||||
@@ -464,6 +76,13 @@ public:
|
||||
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<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
|
||||
* @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> observation_list_;
|
||||
std::list<DepthCameraObservation> 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_
|
||||
|
||||
@@ -46,6 +46,9 @@
|
||||
|
||||
#include <robot_nav_msgs/OccupancyGrid.h>
|
||||
|
||||
#include <mutex>
|
||||
|
||||
#include <robot_sensor_msgs/DepthCameraData.h>
|
||||
#include <robot_sensor_msgs/LaserScan.h>
|
||||
#include <robot_laser_geometry/laser_geometry.hpp>
|
||||
#include <robot_sensor_msgs/PointCloud.h>
|
||||
@@ -128,6 +131,12 @@ protected:
|
||||
void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
|
||||
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
|
||||
* @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;
|
||||
|
||||
/**
|
||||
* @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
|
||||
* @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> > 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
|
||||
std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
|
||||
|
||||
@@ -178,6 +197,10 @@ protected:
|
||||
|
||||
int combination_method_;
|
||||
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:
|
||||
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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,13 +172,41 @@ 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 (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");
|
||||
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
|
||||
}
|
||||
|
||||
if(!frustum_clearing_enabled)
|
||||
{
|
||||
|
||||
CallBackInfo info_tmp;
|
||||
info_tmp.observation_source = source;
|
||||
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;
|
||||
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;
|
||||
|
||||
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
|
||||
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<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;
|
||||
|
||||
int size_callback = static_cast<int>(callback_infos_.size());
|
||||
@@ -287,6 +404,7 @@ void ObstacleLayer::handleImpl(const void* data,
|
||||
// }
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
robot::log_info("Stop receiving data!\n");
|
||||
@@ -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<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,
|
||||
double* min_y, double* max_x, double* max_y)
|
||||
{
|
||||
@@ -550,6 +677,23 @@ bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_o
|
||||
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,
|
||||
double* max_x, double* max_y)
|
||||
{
|
||||
|
||||
@@ -37,7 +37,14 @@
|
||||
*********************************************************************/
|
||||
#include <robot_costmap_2d/voxel_layer.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 <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
|
||||
#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<Observation> observations, clearing_observations;
|
||||
std::vector<DepthCameraObservation> 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<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)
|
||||
// {
|
||||
// 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)
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user