/********************************************************************* * * 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 #include #include #include #include #include #include #include #include #include #include #include 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, unsigned int u_offset, unsigned int v_offset); 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 cell_last_marked_; robot_sensor_msgs::PointCloud clearing_endpoints_; std::vector rolling_costmap_scratch_; std::vector rolling_voxel_scratch_; std::vector rolling_stamp_scratch_; struct DepthRay { unsigned int u; unsigned int v; double x; double y; double z; }; std::vector 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; unsigned int cached_u_offset_ = 0; unsigned int cached_v_offset_ = 0; /// Advances every frustum pass to dither the sampled pixel grid. A static /// camera otherwise re-traces the same fixed pixel_step subgrid each frame, /// so cells between adjacent rays (angular gap pixel_step / fx) are never /// crossed and stay marked until the robot moves. Cycling the grid phase /// sweeps every pixel over pixel_step^2 frames at unchanged per-frame cost. unsigned int frustum_phase_ = 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_