optimal

include/custom_planner/custom_planner.h

@@ -30,17 +30,7 @@ using namespace std;
 #include "custom_planner/merge_path_calc.h"
 
 #include <robot_geometry_msgs/PoseArray.h>
-
-#include <thread>
-#include <boost/thread.hpp>
-
 #include <robot_protocol_msgs/Order.h>
-// #include "vda5050_msgs/Order.h"
-// #include "vda5050_msgs/Trajectory.h"
-// #include "vda5050_msgs/Edge.h"
-// #include "vda5050_msgs/Node.h"
-// #include "vda5050_msgs/ControlPoint.h"
-// #include "vda5050_msgs/NodePosition.h"
 
 const double EPSILON = 1e-6;
 
@@ -115,15 +105,6 @@ private:
 
   bool countRobotDirectionChangeAngle(vector<Pose>& savePosesOnEdge, int& total_edge);
 
-  inline double calculateAngle(double xA, double yA, double xB, double yB) 
-  {
-    double deltaX = xB - xA;
-    double deltaY = yB - yA;
-    double angleRad = atan2(deltaY, deltaX);
-    // double angleDeg = angleRad * 180.0 / M_PI;
-    return angleRad;
-  }
-
   inline bool isOppositeSign(double angleA, double angleB) 
   {
     return (angleA > 0 && angleB < 0) || (angleA < 0 && angleB > 0);
@@ -173,41 +154,12 @@ private:
   bool skipEdge_flag_ = false;
   bool reverse_ = false;
 
-  Spline_Inf* input_spline_inf;
-  Curve_common* CurveDesign;
-  Pathway* pathway;   
-  Pose* startPose;
-  vector<Pose> posesOnPathWay;      
-  Pose start_on_path;  
-  std::map<string, OrderNode> orderNodes;  
+  Spline_Inf* input_spline_inf_;
+  Curve_common* CurveDesign_;
+  vector<Pose> posesOnPathWay_; 
 
-  // vda5050_msgs::Order order_msg_;
-  uint16_t start_on_path_index;
   bool initialized_;
-
   bool rotating_robot_plag_ = false; /**< whether the robot is rotating or not, used to determine the direction of the path */
-  
-  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_;
 
   std::string name_;
   robot_costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
@@ -215,13 +167,6 @@ private:
   unsigned int current_env_width_;
   unsigned int current_env_height_;
 
-  // ros::Subscriber order_msg_sub_;
-  // ros::Publisher plan_pub_;
-  // ros::Publisher stats_publisher_;
-
-  // vector<ros::ServiceServer> service_servers_;
-  
-  // ros::Publisher sbpl_plan_footprint_pub_;  
   boost::mutex mutex_;
 };
 }

include/custom_planner/custom_planner_config.h

@@ -0,0 +1,12 @@
+#ifndef CUSTOM_PLANNER_CONFIG_H_
+#define CUSTOM_PLANNER_CONFIG_H_
+
+#include <robot/robot.h>
+
+namespace custom_planner
+{
+    // struct 
+
+}
+
+#endif // CUSTOM_PLANNER_CONFIG_H_