obstacle_layer.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, 2013, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * Author: Eitan Marder-Eppstein
00036  *         David V. Lu!!
00037  *********************************************************************/
00038 #ifndef COSTMAP_2D_OBSTACLE_LAYER_H_
00039 #define COSTMAP_2D_OBSTACLE_LAYER_H_
00040 
00041 #include <ros/ros.h>
00042 #include <costmap_2d/costmap_layer.h>
00043 #include <costmap_2d/layered_costmap.h>
00044 #include <costmap_2d/observation_buffer.h>
00045 
00046 #include <nav_msgs/OccupancyGrid.h>
00047 
00048 #include <sensor_msgs/LaserScan.h>
00049 #include <laser_geometry/laser_geometry.h>
00050 #include <sensor_msgs/PointCloud.h>
00051 #include <sensor_msgs/PointCloud2.h>
00052 #include <sensor_msgs/point_cloud_conversion.h>
00053 #include <tf/message_filter.h>
00054 #include <message_filters/subscriber.h>
00055 #include <dynamic_reconfigure/server.h>
00056 #include <costmap_2d/ObstaclePluginConfig.h>
00057 #include <costmap_2d/footprint.h>
00058 
00059 namespace costmap_2d
00060 {
00061 
00062 class ObstacleLayer : public CostmapLayer
00063 {
00064 public:
00065   ObstacleLayer()
00066   {
00067     costmap_ = NULL;  // this is the unsigned char* member of parent class Costmap2D.
00068   }
00069 
00070   virtual ~ObstacleLayer();
00071   virtual void onInitialize();
00072   virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
00073                             double* max_x, double* max_y);
00074   virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
00075 
00076   virtual void activate();
00077   virtual void deactivate();
00078   virtual void reset();
00079 
00085   void laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
00086                          const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
00087 
00093   void laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& message,
00094                                  const boost::shared_ptr<ObservationBuffer>& buffer);
00095 
00101   void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
00102                           const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
00103 
00109   void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
00110                            const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
00111 
00112   // for testing purposes
00113   void addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing);
00114   void clearStaticObservations(bool marking, bool clearing);
00115 
00116 protected:
00117   virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
00118 
00124   bool getMarkingObservations(std::vector<costmap_2d::Observation>& marking_observations) const;
00125 
00131   bool getClearingObservations(std::vector<costmap_2d::Observation>& clearing_observations) const;
00132 
00141   virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
00142                                  double* max_x, double* max_y);
00143 
00144   void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
00145                             double* max_x, double* max_y);
00146 
00147   std::vector<geometry_msgs::Point> transformed_footprint_;
00148   bool footprint_clearing_enabled_;
00149   void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, 
00150                        double* max_x, double* max_y);
00151 
00152   std::string global_frame_;  
00153   double max_obstacle_height_;  
00154 
00155   laser_geometry::LaserProjection projector_;  
00156 
00157   std::vector<boost::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_;  
00158   std::vector<boost::shared_ptr<tf::MessageFilterBase> > observation_notifiers_;  
00159   std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_;  
00160   std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_;  
00161   std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_;  
00162 
00163   // Used only for testing purposes
00164   std::vector<costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
00165 
00166   bool rolling_window_;
00167   dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig> *dsrv_;
00168 
00169   int combination_method_;
00170 
00171 private:
00172   void reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level);
00173 };
00174 
00175 }  // namespace costmap_2d
00176 
00177 #endif  // COSTMAP_2D_OBSTACLE_LAYER_H_


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:21