sbpl_lattice_planner.h

Go to the documentation of this file.

#ifndef SBPL_LATTICE_PLANNER_H
#define SBPL_LATTICE_PLANNER_H
 
#include <iostream>
#include <vector>
 
using namespace std;
 
// ROS
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
 
// Costmap used for the map representation
#include <costmap_2d/costmap_2d_ros.h>
 
// sbpl headers
#include <sbpl/headers.h>
 
// global representation
#include <nav_core/base_global_planner.h>
 
namespace sbpl_lattice_planner{
 
class SBPLLatticePlanner : public nav_core::BaseGlobalPlanner{
public:
  
  SBPLLatticePlanner();
 
  
  SBPLLatticePlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
 
 
  virtual void initialize(std::string name, 
                          costmap_2d::Costmap2DROS* costmap_ros);
  
  virtual bool makePlan(const geometry_msgs::PoseStamped& start, 
                        const geometry_msgs::PoseStamped& goal, 
                        std::vector<geometry_msgs::PoseStamped>& plan);
 
  virtual ~SBPLLatticePlanner(){};
 
private:
  unsigned char costMapCostToSBPLCost(unsigned char newcost);
  void publishStats(int solution_cost, int solution_size, 
                    const geometry_msgs::PoseStamped& start, 
                    const geometry_msgs::PoseStamped& goal);
 
  unsigned char computeCircumscribedCost();
 
  static void transformFootprintToEdges(const geometry_msgs::Pose& robot_pose,
                                        const std::vector<geometry_msgs::Point>& footprint,
                                        std::vector<geometry_msgs::Point>& out_footprint);
 
  void getFootprintList(const std::vector<EnvNAVXYTHETALAT3Dpt_t>& sbpl_path, const std::string& path_frame_id,
                        visualization_msgs::Marker& ma);
 
  bool initialized_;
 
  SBPLPlanner* planner_;
  EnvironmentNAVXYTHETALAT* env_;
  
  std::string planner_type_; 
  double allocated_time_; 
  double initial_epsilon_; 
  std::string environment_type_; 
  std::string cost_map_topic_; 
  bool forward_search_; 
  std::string primitive_filename_; 
  int force_scratch_limit_; 
  unsigned char lethal_obstacle_;
  unsigned char inscribed_inflated_obstacle_;
  unsigned char circumscribed_cost_;
  unsigned char sbpl_cost_multiplier_;
 
  bool publish_footprint_path_;
  int visualizer_skip_poses_;
 
  bool allow_unknown_;
 
  std::string name_;
  costmap_2d::Costmap2DROS* costmap_ros_; 
  std::vector<geometry_msgs::Point> footprint_;
  unsigned int current_env_width_;
  unsigned int current_env_height_;
 
  ros::Publisher plan_pub_;
  ros::Publisher stats_publisher_;
  ros::Publisher sbpl_plan_footprint_pub_;
};
};
 
#endif