first commit

include/sbpl_lattice_planner/sbpl_lattice_planner.h

@@ -0,0 +1,123 @@
+#ifndef SBPL_LATTICE_PLANNER_H
+#define SBPL_LATTICE_PLANNER_H
+
+#include <iostream>
+#include <vector>
+
+using namespace std;
+
+// ROBOT
+#include <robot/robot.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_visualization_msgs/Marker.h>
+
+// Costmap used for the map representation
+#include <robot_costmap_2d/costmap_2d_robot.h>
+
+// sbpl headers
+#include <sbpl/headers.h>
+
+// global representation
+#include <robot_nav_core/base_global_planner.h>
+
+namespace sbpl_lattice_planner{
+
+class SBPLLatticePlanner : public robot_nav_core::BaseGlobalPlanner{
+public:
+  
+  /**
+   * @brief  Default constructor for the NavFnROBOT object
+   */
+  SBPLLatticePlanner();
+
+  
+  /**
+   * @brief  Constructor for the SBPLLatticePlanner object
+   * @param  name The name of this planner
+   * @param  costmap_robot A pointer to the ROBOT wrapper of the costmap to use
+   */
+  SBPLLatticePlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot);
+
+
+  /**
+   * @brief  Initialization function for the SBPLLatticePlanner object
+   * @param  name The name of this planner
+   * @param  costmap_robot A pointer to the ROBOT wrapper of the costmap to use
+   */
+  virtual bool initialize(std::string name, 
+                          robot_costmap_2d::Costmap2DROBOT* costmap_robot);
+  
+  /**
+   * @brief Given a goal pose in the world, compute a plan
+   * @param start The start pose 
+   * @param goal The goal pose 
+   * @param plan The plan... filled by the planner
+   * @return True if a valid plan was found, false otherwise
+   */
+  virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start, 
+                        const robot_geometry_msgs::PoseStamped& goal, 
+                        std::vector<robot_geometry_msgs::PoseStamped>& plan);
+
+  virtual ~SBPLLatticePlanner(){};
+  
+  static robot_nav_core::BaseGlobalPlanner::Ptr create();
+
+private:
+  unsigned char costMapCostToSBPLCost(unsigned char newcost);
+
+  unsigned char computeCircumscribedCost();
+
+  static void transformFootprintToEdges(const robot_geometry_msgs::Pose& robot_pose,
+                                        const std::vector<robot_geometry_msgs::Point>& footprint,
+                                        std::vector<robot_geometry_msgs::Point>& out_footprint);
+
+  void getFootprintList(const std::vector<EnvNAVXYTHETALAT3Dpt_t>& sbpl_path, const std::string& path_frame_id,
+                        robot_visualization_msgs::Marker& ma);
+
+  bool initialized_;
+
+  SBPLPlanner* planner_;
+  EnvironmentNAVXYTHETALAT* env_;
+  
+  std::string planner_type_; /**< sbpl method to use for planning.  choices are ARAPlanner and ADPlanner */
+
+  double allocated_time_; /**< amount of time allowed for search */
+  double initial_epsilon_; /**< initial epsilon for beginning the anytime search */
+
+  std::string environment_type_; /** what type of environment in which to plan.  choices are 2D and XYThetaLattice. */ 
+  std::string cost_map_topic_; /** what topic is being used for the costmap topic */
+
+  bool forward_search_; /** whether to use forward or backward search */
+  std::string primitive_filename_; /** where to find the motion primitives for the current robot */
+  int force_scratch_limit_; /** the number of cells that have to be changed in the costmap to force the planner to plan from scratch even if its an incremental planner */
+
+  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_;
+
+  /** Bỏ ràng buộc heading tại điểm xuất phát, đảm bảo path MỞ ĐẦU BẰNG ĐOẠN THẲNG
+   *  theo hướng đi thật của route (không nhất thiết hướng tới goal). Dùng khi local
+   *  planner tự quay tại chỗ về hướng path trước khi bám (turn_around_priority) — khi
+   *  đó heading xuất phát không phải ràng buộc thật, giữ nó chỉ ép SBPL vẽ cung quay
+   *  đầu lúc goal nằm phía sau robot. Cơ chế: plan lần 1 với heading seed hướng tới
+   *  goal; nếu đoạn đầu vẫn cong thì căn heading theo đoạn thẳng đầu tiên của path và
+   *  plan lại một lần. Không ra nghiệm thì retry với heading thật. */
+  bool free_start_heading_;
+
+  std::string name_;
+  robot_costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
+  std::vector<robot_geometry_msgs::Point> footprint_;
+  std::vector<robot_geometry_msgs::Point> footprint_prev_;
+  unsigned int current_env_width_;
+  unsigned int current_env_height_;
+};
+};
+
+#endif
+