193 lines
7.5 KiB
C++
Executable File
193 lines
7.5 KiB
C++
Executable File
/*********************************************************************
|
|
*
|
|
* 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
|
|
* David V. Lu!!
|
|
*********************************************************************/
|
|
#ifndef ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
|
|
#define ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
|
|
|
|
#include <robot_costmap_2d/layer.h>
|
|
#include <robot_costmap_2d/layered_costmap.h>
|
|
#include <robot_costmap_2d/observation_buffer.h>
|
|
#include <robot_costmap_2d/voxel_grid.h>
|
|
#include <robot_nav_msgs/OccupancyGrid.h>
|
|
#include <robot_sensor_msgs/LaserScan.h>
|
|
#include <robot_laser_geometry/laser_geometry.hpp>
|
|
#include <robot_sensor_msgs/PointCloud.h>
|
|
#include <robot_sensor_msgs/PointCloud2.h>
|
|
#include <robot_sensor_msgs/point_cloud_conversion.h>
|
|
#include <robot_costmap_2d/obstacle_layer.h>
|
|
#include <robot_voxel_grid/voxel_grid.h>
|
|
|
|
namespace robot_costmap_2d
|
|
{
|
|
|
|
class VoxelLayer : public ObstacleLayer
|
|
{
|
|
public:
|
|
VoxelLayer() :
|
|
robot_voxel_grid_(0, 0, 0)
|
|
{
|
|
costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
|
|
}
|
|
|
|
virtual ~VoxelLayer();
|
|
|
|
virtual void onInitialize();
|
|
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
|
|
double* max_x, double* max_y);
|
|
|
|
void updateOrigin(double new_origin_x, double new_origin_y);
|
|
bool isDiscretized()
|
|
{
|
|
return true;
|
|
}
|
|
virtual void matchSize();
|
|
virtual void reset();
|
|
|
|
LayerType getType() const override
|
|
{
|
|
return LayerType::VOXEL_LAYER;
|
|
}
|
|
|
|
protected:
|
|
|
|
virtual void resetMaps();
|
|
|
|
private:
|
|
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
|
|
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(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;
|
|
void updateDepthRayCache(unsigned int width, unsigned int height, unsigned int pixel_step,
|
|
double fx, double fy, double cx, double cy);
|
|
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, unsigned int cell_raytrace_range,
|
|
double* min_x, double* min_y, double* max_x, double* max_y);
|
|
/// Frees LETHAL cells this layer marked that have not been re-marked within
|
|
/// obstacle_decay_time. Handles ghost cells frustum clearing can never
|
|
/// reach: cells that left the camera FOV, occlusion shadows, and cells
|
|
/// inside the skip band in front of a measured surface.
|
|
void decayStaleObstacles(double now_sec, double* min_x, double* min_y,
|
|
double* max_x, double* max_y);
|
|
|
|
|
|
bool publish_voxel_;
|
|
robot_voxel_grid::VoxelGrid robot_voxel_grid_;
|
|
double z_resolution_, origin_z_;
|
|
unsigned int unknown_threshold_, mark_threshold_, size_z_;
|
|
/// Seconds a marked cell survives without being re-observed before it is
|
|
/// freed. <= 0 disables decay (default). Only enable on camera-only
|
|
/// costmaps: decay clears obstacles the sensor cannot currently see.
|
|
double obstacle_decay_time_{0.0};
|
|
/// Clearing rays stop this far [m] before the measured surface. < 0 keeps
|
|
/// the legacy 2 * resolution behaviour.
|
|
double frustum_skip_distance_{-1.0};
|
|
/// Per-cell robot::Time seconds of the last marking; 0 = no valid stamp.
|
|
std::vector<double> cell_last_marked_;
|
|
robot_sensor_msgs::PointCloud clearing_endpoints_;
|
|
std::vector<unsigned char> rolling_costmap_scratch_;
|
|
std::vector<unsigned int> rolling_voxel_scratch_;
|
|
std::vector<double> rolling_stamp_scratch_;
|
|
|
|
struct DepthRay
|
|
{
|
|
unsigned int u;
|
|
unsigned int v;
|
|
double x;
|
|
double y;
|
|
double z;
|
|
};
|
|
std::vector<DepthRay> depth_ray_cache_;
|
|
unsigned int cached_depth_width_ = 0;
|
|
unsigned int cached_depth_height_ = 0;
|
|
unsigned int cached_depth_pixel_step_ = 0;
|
|
double cached_fx_ = 0.0;
|
|
double cached_fy_ = 0.0;
|
|
double cached_cx_ = 0.0;
|
|
double cached_cy_ = 0.0;
|
|
|
|
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
|
|
{
|
|
if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
|
|
return false;
|
|
mx = ((wx - origin_x_) / resolution_);
|
|
my = ((wy - origin_y_) / resolution_);
|
|
mz = ((wz - origin_z_) / z_resolution_);
|
|
if (mx < size_x_ && my < size_y_ && mz < size_z_)
|
|
return true;
|
|
|
|
return false;
|
|
}
|
|
|
|
inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz)
|
|
{
|
|
if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
|
|
return false;
|
|
|
|
mx = (int)((wx - origin_x_) / resolution_);
|
|
my = (int)((wy - origin_y_) / resolution_);
|
|
mz = (int)((wz - origin_z_) / z_resolution_);
|
|
|
|
if (mx < size_x_ && my < size_y_ && mz < size_z_)
|
|
return true;
|
|
|
|
return false;
|
|
}
|
|
|
|
inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz)
|
|
{
|
|
// returns the center point of the cell
|
|
wx = origin_x_ + (mx + 0.5) * resolution_;
|
|
wy = origin_y_ + (my + 0.5) * resolution_;
|
|
wz = origin_z_ + (mz + 0.5) * z_resolution_;
|
|
}
|
|
|
|
inline double dist(double x0, double y0, double z0, double x1, double y1, double z1)
|
|
{
|
|
return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
|
|
}
|
|
};
|
|
|
|
} // namespace robot_costmap_2d
|
|
|
|
#endif // ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
|