Add files via upload

exploration/include/edge_follow_params.hpp

@@ -25,15 +25,15 @@ struct PlanEdgeFollowParams
     bool use_terrain_analysis_ = false;
     bool check_obstacle_ = true;
     bool check_rot_obstacle_ = true;
-    double adjacent_range_ = 1.0;
+    double adjacent_range_ = 2.0;
     double obstacle_height_thre_ = 0.2;
     double ground_height_thre_ = 0.1;
     double cost_height_thre_ = 0.1;
     double cost_score_ = 0.02;
     bool use_cost_ = false;
     const int laser_cloud_stack_num_ = 1;
     int laser_cloud_count_ = 0;
-    int point_per_path_thre_ = 2;
+    int point_per_path_thre_ = 1;
     double min_rel_z_ = -0.5;
     double max_rel_z_ = 0.25;
     double max_speed_ = 1.0;
@@ -56,8 +56,8 @@ struct PlanEdgeFollowParams
 
 
     //路径相关
-    static constexpr int path_num_ = 343; //路径的个数
-    static constexpr int group_num_ = 7;
+    static constexpr int path_num_ = 361;//343 //路径的个数
+    static constexpr int group_num_ = 19;//7
     float grid_voxel_size_ = 0.02;
     float search_radius_ = 0.45;//搜索半径
     float grid_voxel_offset_x_ = 2;//网格x偏移
@@ -70,17 +70,16 @@ struct PlanEdgeFollowParams
     std::vector<Point2f> planner_cloud_crop_;
     std::vector<std::vector<Point2f>> start_paths_ = std::vector<std::vector<Point2f>>(group_num_);
 
-    #if PLOTPATHSET == 1
     std::vector<std::vector<Point2f>> paths_ = std::vector<std::vector<Point2f>>(path_num_);
     std::vector<Point2f> free_paths_;
-    #endif
 
 
     int path_list_[path_num_] = {0};
     float end_dir_path_list_[path_num_] = {0};
-    int clear_path_list_[36 * path_num_] = {0};
-    float path_penalty_list_[36 * path_num_] = {0};
-    float clear_path_per_group_score_[36 * group_num_] = {0};
+    int clear_path_list_[5 * path_num_] = {0};
+    float path_penalty_list_[5 * path_num_] = {0};
+    float clear_path_per_group_score_[5 * group_num_] = {0};
+    float clear_path_score_[5 * path_num_] = {0};
     std::vector<int> correspondences[grid_voxel_num_];
 
 

exploration/include/local_planner.h

@@ -39,6 +39,8 @@
 #include "plan_edge_follow.hpp"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
+#include "std_msgs/msg/int8.hpp"
+#include <random>
 /*
     1. 订阅点云信息并做过滤
     2. 订阅odom信息
@@ -60,10 +62,13 @@ class LocalPlanner {
   void pub_local_path();
   void plan();
   void pub_path();
-  //   void pub_obs();
+  void pub_path(Eigen::Vector3d local_odom_pos);
+
+  void pub_obs();
   void pub_res(Eigen::Vector3d local_odom_pos);
   void pub_odom_res(Eigen::Vector3d local_odom_pos);
   void pub_goal();
+  void pub_stop();
 
   ~LocalPlanner() = default;
 
@@ -73,7 +78,7 @@ class LocalPlanner {
 
   // String
   std::string sub_state_estimation_topic_{"/odom"};
-  std::string sub_point_cloud_topic_{"/point_cloud"};
+  std::string sub_point_cloud_topic_{"/point_cloud"};//point_cloud
   rclcpp::Node::SharedPtr nh_;
   //ROS timer
   rclcpp::TimerBase::SharedPtr planner_timer_;
@@ -95,6 +100,7 @@ class LocalPlanner {
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_local_path_;
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_odom_path_;
   rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr pub_static_goal_;
+  rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr pub_stop_signal_;
 
   //tf2
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
@@ -106,5 +112,6 @@ class LocalPlanner {
   nav_msgs::msg::Path global_path_;
   bool has_global_path_{false};
   std::mutex odom_mutex_;
-
+  float end_x_;
+  float end_y_;
 };

exploration/include/plan_edge_follow.hpp

@@ -9,4 +9,7 @@ void ReadPaths(EdgeFollowContext& context_data);
 void ReadPathList(EdgeFollowContext& context_data);
 void ReadCorrespondences(EdgeFollowContext& context_data);
 void GoalHandler(EdgeFollowContext& context_data,Eigen::Vector3d odom_pose);
-void PathSelection(EdgeFollowContext& context_data);
+void PathSelection(EdgeFollowContext& context_data);
+void PathSelection(EdgeFollowContext& context_data,Eigen::Vector3d odom_pos);
+float PointToSegmentDistance(float px, float py,float x1, float y1,float x2, float y2);
+float ComputeDistanceToPath(float x,float y,const std::vector<PointXYYAW>& global_path);