costmap_2d_ros.h

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 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 the Willow Garage 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 COSTMAP_COSTMAP_2D_ROS_H_
#define COSTMAP_COSTMAP_2D_ROS_H_

#include <XmlRpc.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_publisher.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/voxel_costmap_2d.h>
#include <costmap_2d/VoxelGrid.h>
#include <nav_msgs/OccupancyGrid.h>
#include <map>
#include <vector>
#include <string>
#include <sstream>
#include <boost/tokenizer.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>

#include <tf/transform_datatypes.h>

#include <tf/message_filter.h>
#include <message_filters/subscriber.h>

#include <tf/transform_listener.h>

#include <sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.h>

#include <sensor_msgs/PointCloud.h>

//Support for PointCloud2 messages
#include <sensor_msgs/PointCloud2.h>

// Thread suppport
#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>

#include <stdlib.h>
#include <dynamic_reconfigure/server.h>
#include <costmap_2d/Costmap2DConfig.h>

namespace costmap_2d {

  class Costmap2DROS {
    public:
      Costmap2DROS(std::string name, tf::TransformListener& tf);

      ~Costmap2DROS();

      void addObservationBuffer(const boost::shared_ptr<ObservationBuffer>& buffer);

      bool getMarkingObservations(std::vector<Observation>& marking_observations) const;

      bool getClearingObservations(std::vector<Observation>& clearing_observations) const;

      void updateMap();

      void getOrientedFootprint(double x, double y, double theta, std::vector<geometry_msgs::Point>& oriented_footprint) const;

      void getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const;

      bool getRobotPose(tf::Stamped<tf::Pose>& global_pose) const;

      void clearRobotFootprint();

      void clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose);

      bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);

      void resetMapOutsideWindow(double size_x, double size_y);

      void clearNonLethalWindow(double size_x, double size_y);

      void getCostmapCopy(Costmap2D& costmap) const;

      void updateStaticMap(const nav_msgs::OccupancyGrid& new_map);

      void getCostmapWindowCopy(double win_size_x, double win_size_y, Costmap2D& costmap) const;

      void getCostmapWindowCopy(double win_center_x, double win_center_y, double win_size_x, double win_size_y, Costmap2D& costmap) const;

      unsigned int getSizeInCellsX() const;

      unsigned int getSizeInCellsY() const;

      double getResolution() const;

      std::string getGlobalFrameID() const;

      std::string getBaseFrameID() const;

      double getInscribedRadius() const;

      double getCircumscribedRadius() const;

      double getInflationRadius() const;

      std::vector<geometry_msgs::Point> getRobotFootprint() const;

      bool isCurrent() {return current_;}

      void start();

      void stop();


      void pause() {
        stop_updates_ = true;
        initialized_ = false;
      }


      void resume(){
        stop_updates_ = false;

        //block until the costmap is re-initialized.. meaning one update cycle has run
        ros::Rate r(100.0);
        while(!initialized_)
          r.sleep();
      }

    private:
      void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level);

      void movementCB(const ros::TimerEvent &event);

      void incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map);

      void initFromMap(const nav_msgs::OccupancyGrid& map);

      void laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, const boost::shared_ptr<ObservationBuffer>& buffer);

      void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message, const boost::shared_ptr<ObservationBuffer>& buffer);

      void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message, const boost::shared_ptr<ObservationBuffer>& buffer);

      void mapUpdateLoop(double frequency);

      std::vector<geometry_msgs::Point> loadRobotFootprint(ros::NodeHandle node, double inscribed_radius, double circumscribed_radius);

      double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1);

      inline double distance(double x0, double y0, double x1, double y1){
        return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0));
      }

      std::string name_;
      tf::TransformListener& tf_; 
      laser_geometry::LaserProjection projector_; 
      Costmap2D* costmap_; 
      std::string global_frame_; 
      std::string robot_base_frame_; 
      boost::thread* map_update_thread_; 

      std::vector<boost::shared_ptr<tf::MessageFilterBase> > observation_notifiers_; 
      std::vector<boost::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_; 
      std::vector<boost::shared_ptr<ObservationBuffer> > observation_buffers_; 
      std::vector<boost::shared_ptr<ObservationBuffer> > marking_buffers_; 
      std::vector<boost::shared_ptr<ObservationBuffer> > clearing_buffers_; 
      bool rolling_window_; 
      bool current_; 
      double transform_tolerance_; // timeout before transform errors
      Costmap2DPublisher* costmap_publisher_;
      bool stop_updates_, initialized_, stopped_;
      bool publish_voxel_;
      std::vector<geometry_msgs::Point> footprint_spec_;
      ros::Publisher voxel_pub_;
      mutable boost::recursive_mutex lock_;
      bool map_update_thread_shutdown_;
      bool save_debug_pgm_;
      ros::Subscriber map_sub_;
      bool map_initialized_;
      std::string tf_prefix_;

      //we need this to be able to initialize the map using a latched topic approach
      //strictly speaking, we don't need the lock, but since this all happens on startup
      //and there is little overhead... we'll be careful and use it anyways just in case
      //the compiler or scheduler does something weird with the code
      boost::recursive_mutex map_data_lock_;
      nav_msgs::MapMetaData map_meta_data_;
      std::vector<unsigned char> input_data_;
      bool costmap_initialized_;

      bool robot_stopped_, setup_, static_map_;

      dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
      boost::mutex map_update_mutex_;
      boost::recursive_mutex configuration_mutex_;
      costmap_2d::Costmap2DConfig last_config_;
      costmap_2d::Costmap2DConfig maptype_config_;
      costmap_2d::Costmap2DConfig default_config_;
      ros::Timer timer_;
      tf::Stamped<tf::Pose> old_pose_;
  };
};

#endif