VFH无人机避障算法代码移植

/*************************************************************************************************************************** * * Author: bingo * Email: 1554459957@qq.com * Time: 2019.10.14 * Description: lidar collision vfh v1.0 * ***************************************************************************************************************************/ //ROS 头文件 #include <ros/ros.h> #include <Eigen/Eigen> #include <cmath> #include <algorithm> #include <vector> //topic 头文件 #include <iostream> #include<stdio.h> #include <std_msgs/Bool.h> #include <mavros_msgs/State.h> #include <geometry_msgs/Pose.h> #include <sensor_msgs/LaserScan.h> #include <geometry_msgs/PoseStamped.h> #include <mavros_msgs/PositionTarget.h> #include <eigen_conversions/eigen_msg.h> #include <mavros/frame_tf.h> #include <mavros_msgs/WaypointList.h> #include <std_msgs/Float64.h> #include <mavros_msgs/HomePosition.h> #include <GeographicLib/Geocentric.hpp> #include <sensor_msgs/NavSatFix.h> #include "tf/transform_datatypes.h"//转换函数头文件 #include <nav_msgs/Odometry.h>//里程计信息格式 using namespace std; #define PI 3.1415926 struct Point2D { float x; float y; float z; }; float scan_distance_max; float scan_distance_min; float angle_resolution; float heading; float sector_value; float sector_scale; Point2D Uavp; vector<float> map_cv; vector<double> ranges; float desire_z = 1.5; //期望高度 uint32_t init_mask = 0; Eigen::Vector3d vel_sp_body; void SetUavPosition(Point2D& uav) { Uavp.x = uav.x; Uavp.y = uav.y; Uavp.z = uav.z; } void SetUavHeading(float hd) { heading = hd; heading += 270; heading = (int)heading % 360; heading = 360 - heading; } void ComputeMV(vector<float> r) { float dist[360] = { 0 }; ranges.clear(); map_cv.clear(); int range_size = r.size(); //有多少条激光 for (size_t i = 0; i < range_size; i++) { //A non-zero value (true) if x is a NaN value; and zero (false) otherwise. //isinf A non-zero value (true) if x is an infinity; and zero (false) otherwise. if (!std::isnan(r[i]) && !std::isinf(r[i]))//取出有效值 { float scan_distance = r[i]; int sector_index = std::floor((i*angle_resolution) / sector_value);//(i*1)/30 sector_index:[1 12] if (scan_distance >= scan_distance_max || scan_distance < scan_distance_min) scan_distance = 0; else scan_distance = scan_distance_max - scan_distance; dist[sector_index] += scan_distance; } ranges.push_back(r[i]); } for (int j = 0; j < (int)(360 / sector_value); j++)//把uav四周分为12个sector，每个sector的值越小，代表越安全。 { map_cv.push_back(dist[j]); // printf("dist[%d]=%f-",j,dist[j]); } // printf("##############################################################################################\n"); } /*检测前方[340 360],[0 20]范围内是否安全*/ bool IsFrontSafety() { float goal_sector = (int)(0 - (sector_value - sector_scale) + 360) % 360;//(0-(30-10)+360)%360 = 340,goal_sector = 340 int start_index = goal_sector / angle_resolution;//start_index = 340 float scan_distance = 0; for (int i = 0; i < (sector_value - sector_scale) * 2 / angle_resolution; i++)//for(i=0;i<40;i++) { int real_index = (start_index + i) % (int)(360 / angle_resolution); if (!std::isnan(ranges[real_index]) && !std::isinf(ranges[real_index])) { if (ranges[real_index] < scan_distance_max && ranges[real_index] >= scan_distance_min) scan_distance = scan_distance_max - ranges[real_index] + scan_distance; } } if (scan_distance < 0.1) { return true; } return false; } /*输入期望坐标点，输出期望机头方向*/ float CalculDirection(Point2D& goal) { float ori; //Compute arc tangent with two parameters //return Principal arc tangent of y/x, in the interval [-pi,+pi] radians. //One radian is equivalent to 180/PI degrees. float G_theta = atan2((goal.y - Uavp.y), (goal.x - Uavp.x)); float goal_ori = G_theta * 180 / PI; //目标点相对uav的方向信息，即与y轴的夹角，正值代表在第一三现象，负值代表在二四现象。ENU坐标系下 if (goal_ori < 0) { goal_ori += 360; } goal_ori -= heading; goal_ori += 360; goal_ori = (int)goal_ori % 360; //把目标点方向转化成机体坐标系下的方向 float goal_sector = (int)(goal_ori - sector_value + 360) % 360; int start_index = goal_sector / angle_resolution; float scan_distance = 0; for (int i = 0; i < sector_value * 2 / angle_resolution; i++) { int real_index = (start_index + i) % (int)(360 / angle_resolution); if (!std::isnan(ranges[real_index]) && !std::isinf(ranges[real_index])) { if (ranges[real_index] < scan_distance_max && ranges[real_index] >= scan_distance_min) scan_distance = scan_distance_max - ranges[real_index] + scan_distance; } } if (scan_distance < 0.1)//判断目标方向处是否安全 { ori = goal_ori; ori += heading; ori = (int)ori % 360; return ori;//若安全，则把机头指向目标处 } vector<int> mesh; for (int i = 0; i < map_cv.size(); i++) //确定栅格中CV值 共有12个栅格，CV值越大，存在障碍物可能性越大 { if (map_cv[i] < 0.1) mesh.push_back(0); else if (map_cv[i] >= 0.1 && map_cv[i] < 0.3) mesh.push_back(2); else mesh.push_back(4); } vector<float> cand_dir; for (int j = 0; j < mesh.size(); j++) { if (j == mesh.size() - 1) //if(j == 11) { if (mesh[0] + mesh[mesh.size() - 1] == 0) cand_dir.push_back(0.0); } else { if (mesh[j] + mesh[j + 1] == 0) cand_dir.push_back((j + 1)*sector_value);//寻找安全角度，机体坐标系下；即确定波谷 } } if (cand_dir.size() != 0) { vector<float> delta; for (auto &dir_ite : cand_dir) { float delte_theta1 = fabs(dir_ite - goal_ori); float delte_theta2 = 360 - delte_theta1; float delte_theta = delte_theta1 < delte_theta2 ? delte_theta1 : delte_theta2; delta.push_back(delte_theta); }//寻找接近目标区域的波谷 int min_index = min_element(delta.begin(), delta.end()) - delta.begin(); ori = cand_dir.at(min_index); ori += heading; ori = (int)ori % 360; return ori;//确定机头方向 } return -1; } mavros_msgs::State current_state; void state_cb(const mavros_msgs::State::ConstPtr& msg) { init_mask |= 1 << 1; current_state = *msg; } mavros_msgs::HomePosition home_pos; void home_pos_cb(const mavros_msgs::HomePosition::ConstPtr& msg) { init_mask |= 1 << 2; home_pos = *msg; } geometry_msgs::PoseStamped local_pos; Eigen::Vector3d current_local_pos; bool local_pos_updated = false; void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) { init_mask |= 1 << 3; current_local_pos = mavros::ftf::to_eigen(msg->pose.position); local_pos = *msg; Point2D pt2d; pt2d.x = current_local_pos.x(); pt2d.y = current_local_pos.y(); SetUavPosition(pt2d); local_pos_updated = true; } /* mavros_msgs::WaypointList waypoints; void waypoints_cb(const mavros_msgs::WaypointList::ConstPtr& msg) { init_mask |= 1 << 4; waypoints = *msg; } */ bool scan_updated = false; void scan_cb(const sensor_msgs::LaserScan::ConstPtr &msg) { //激光雷达数据回调函数 init_mask |= 1 << 5; ComputeMV(msg->ranges); scan_updated = true; } bool heading_updated = false; void heading_cb(const std_msgs::Float64::ConstPtr &msg) { init_mask |= 1 << 6; SetUavHeading(msg->data); heading_updated = true; } Eigen::Vector3d current_gps; void gps_cb(const sensor_msgs::NavSatFix::ConstPtr &msg) { init_mask |= 1; current_gps = { msg->latitude, msg->longitude, msg->altitude }; } //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< int main(int argc, char **argv) { ros::init(argc, argv, "collision_avoidance_vfh"); ros::NodeHandle nh("~"); scan_distance_max = 2.1; scan_distance_min = 0.1; angle_resolution = 1.0; heading = 90; sector_value = 30; sector_scale = 10; Uavp.x = 0; Uavp.y = 0; ros::Subscriber gps_sub