178 lines
7.5 KiB
C++
Executable File
178 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_OBSTACLE_LAYER_H_
|
|
#define ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
|
|
|
|
#include <robot_costmap_2d/costmap_layer.h>
|
|
#include <robot_costmap_2d/layered_costmap.h>
|
|
#include <robot_costmap_2d/observation_buffer.h>
|
|
#include <robot_costmap_2d/footprint.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>
|
|
|
|
namespace robot_costmap_2d
|
|
{
|
|
|
|
class ObstacleLayer : public CostmapLayer
|
|
{
|
|
public:
|
|
ObstacleLayer()
|
|
{
|
|
costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D.
|
|
}
|
|
|
|
virtual ~ObstacleLayer();
|
|
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);
|
|
virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
|
|
|
|
virtual void activate();
|
|
virtual void deactivate();
|
|
virtual void reset();
|
|
|
|
// for testing purposes
|
|
void addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing);
|
|
void clearStaticObservations(bool marking, bool clearing);
|
|
|
|
LayerType getType() const override
|
|
{
|
|
return LayerType::OBSTACLE_LAYER;
|
|
}
|
|
|
|
protected:
|
|
void handleImpl(const void* data,
|
|
const std::type_info&,
|
|
const std::string& topic) override;
|
|
|
|
/**
|
|
* @brief A callback to handle buffering LaserScan messages
|
|
* @param message The message returned from a message notifier
|
|
* @param buffer A pointer to the observation buffer to update
|
|
*/
|
|
void laserScanCallback(const robot_sensor_msgs::LaserScan& message,
|
|
const boost::shared_ptr<ObservationBuffer>& buffer);
|
|
|
|
/**
|
|
* @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max.
|
|
* @param message The message returned from a message notifier
|
|
* @param buffer A pointer to the observation buffer to update
|
|
*/
|
|
void laserScanValidInfCallback(const robot_sensor_msgs::LaserScan& message,
|
|
const boost::shared_ptr<ObservationBuffer>& buffer);
|
|
|
|
/**
|
|
* @brief A callback to handle buffering PointCloud messages
|
|
* @param message The message returned from a message notifier
|
|
* @param buffer A pointer to the observation buffer to update
|
|
*/
|
|
void pointCloudCallback(const robot_sensor_msgs::PointCloud& message,
|
|
const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
|
|
|
|
/**
|
|
* @brief A callback to handle buffering PointCloud2 messages
|
|
* @param message The message returned from a message notifier
|
|
* @param buffer A pointer to the observation buffer to update
|
|
*/
|
|
void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& 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
|
|
* @return True if all the observation buffers are current, false otherwise
|
|
*/
|
|
bool getMarkingObservations(std::vector<robot_costmap_2d::Observation>& marking_observations) const;
|
|
|
|
/**
|
|
* @brief Get the observations used to clear space
|
|
* @param clearing_observations A reference to a vector that will be populated with the observations
|
|
* @return True if all the observation buffers are current, false otherwise
|
|
*/
|
|
bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& clearing_observations) const;
|
|
|
|
/**
|
|
* @brief Clear freespace based on one observation
|
|
* @param clearing_observation The observation used to raytrace
|
|
* @param min_x
|
|
* @param min_y
|
|
* @param max_x
|
|
* @param max_y
|
|
*/
|
|
virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
|
|
double* max_x, double* max_y);
|
|
|
|
void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
|
|
double* max_x, double* max_y);
|
|
|
|
std::vector<robot_geometry_msgs::Point> transformed_footprint_;
|
|
bool footprint_clearing_enabled_;
|
|
void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
|
|
double* max_x, double* max_y);
|
|
|
|
std::string global_frame_; ///< @brief The global frame for the costmap
|
|
double max_obstacle_height_; ///< @brief Max Obstacle Height
|
|
|
|
robot_laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
|
|
|
|
std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors
|
|
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
|
|
|
|
// Used only for testing purposes
|
|
std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
|
|
|
|
bool rolling_window_;
|
|
bool stop_receiving_data_ = false;
|
|
|
|
int combination_method_;
|
|
|
|
private:
|
|
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
|
|
};
|
|
|
|
} // namespace robot_costmap_2d
|
|
|
|
#endif // ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
|