遗传算法路径规划的ROS实现.rar

#include <stdio.h> #include <stdlib.h> #include <unistd.h> #include <string.h> #include <sys/types.h> #include <sys/socket.h> #include <netinet/in.h> #include <netdb.h> #include <fstream> #include <iostream> #include <iomanip> #include <string> #include <ctime> #include <math.h> #include "RAstar_ros.h" #include <pluginlib/class_list_macros.h> PLUGINLIB_EXPORT_CLASS(RAstar_planner::RAstarPlannerROS, nav_core::BaseGlobalPlanner) int value; int mapSize; bool* OGM; static const float INFINIT_COST = INT_MAX; float infinity = std::numeric_limits< float >::infinity(); float tBreak; ofstream MyExcelFile ("RA_result.xlsx", ios::trunc); int clock_gettime(clockid_t clk_id, struct timespect *tp); timespec diff(timespec start, timespec end) { timespec temp; if ((end.tv_nsec-start.tv_nsec) < 0) { temp.tv_sec = end.tv_sec-start.tv_sec - 1; temp.tv_nsec = 1000000000 + end.tv_nsec-start.tv_nsec; } else { temp.tv_sec = end.tv_sec-start.tv_sec; temp.tv_nsec = end.tv_nsec-start.tv_nsec; } return temp; } inline vector <int> findFreeNeighborCell (int CellID); namespace RAstar_planner { RAstarPlannerROS::RAstarPlannerROS() { } RAstarPlannerROS::RAstarPlannerROS(ros::NodeHandle &nh) { ROSNodeHandle = nh; } RAstarPlannerROS::RAstarPlannerROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros) { initialize(name, costmap_ros); } void RAstarPlannerROS::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) { if (!initialized_) { costmap_ros_ = costmap_ros; costmap_ = costmap_ros_->getCostmap(); ros::NodeHandle private_nh("~/" + name); originX = costmap_->getOriginX(); originY = costmap_->getOriginY(); width = costmap_->getSizeInCellsX(); height = costmap_->getSizeInCellsY(); resolution = costmap_->getResolution(); mapSize = width*height; tBreak = 1+1/(mapSize); value =0; pc = 0.8; pm = 0.2; max_gen = 50; NP = 200; weight_a = 1; weight_b = 7; OGM = new bool [mapSize]; for (unsigned int iy = 0; iy < costmap_->getSizeInCellsY(); iy++) { for (unsigned int ix = 0; ix < costmap_->getSizeInCellsX(); ix++) { unsigned int cost = static_cast<int>(costmap_->getCost(ix, iy)); if (cost < 250) OGM[iy*width+ix]=true; else OGM[iy*width+ix]=false; } } MyExcelFile << "StartID\tStartX\tStartY\tGoalID\tGoalX\tGoalY\tPlannertime(ms)\tpathLength\tnumberOfCells\t" << endl; ROS_INFO("RAstar planner initialized successfully"); initialized_ = true; } else ROS_WARN("This planner has already been initialized... doing nothing"); } bool RAstarPlannerROS::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) { if (!initialized_) { ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner"); return false; } ROS_DEBUG("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y); plan.clear(); if (goal.header.frame_id != costmap_ros_->getGlobalFrameID()) { ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.", costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str()); return false; } tf::Stamped < tf::Pose > goal_tf; tf::Stamped < tf::Pose > start_tf; poseStampedMsgToTF(goal, goal_tf); poseStampedMsgToTF(start, start_tf); float startX = start.pose.position.x; float startY = start.pose.position.y; float goalX = goal.pose.position.x; float goalY = goal.pose.position.y; getCorrdinate(startX, startY); getCorrdinate(goalX, goalY); int startCell; int goalCell; if (isCellInsideMap(startX, startY) && isCellInsideMap(goalX, goalY)) { startCell = convertToCellIndex(startX, startY); goalCell = convertToCellIndex(goalX, goalY); MyExcelFile << startCell <<"\t"<< start.pose.position.x <<"\t" << start.pose.position.y <<"\t"<< goalCell <<"\t"<< goal.pose.position.x <<"\t"<< goal.pose.position.y; } else { ROS_WARN("the start or goal is out of the map"); return false; } if (isStartAndGoalCellsValid(startCell, goalCell)) { vector<int> bestPath; bestPath.clear(); cout << "width" << width <<"height"<<height<<endl; cout << "originX" << originX <<"originY"<<originY<<"resolution"<< resolution<<endl; bool isFind = RAstarPlanner(startCell, goalCell, bestPath); cout <<"bestPath.size()" << bestPath.size() << endl; if ( bestPath.size()>0) { for (int i = 0; i < bestPath.size(); i++) { float x = 0.0; float y = 0.0; int index = bestPath[i]; convertToCoordinate(index, x, y); geometry_msgs::PoseStamped pose = goal; pose.pose.position.x = x; pose.pose.position.y = y; pose.pose.position.z = 0.0; pose.pose.orientation.x = 0.0; pose.pose.orientation.y = 0.0; pose.pose.orientation.z = 0.0; pose.pose.orientation.w = 1.0; plan.push_back(pose); } float path_length = 0.0; std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin(); geometry_msgs::PoseStamped last_pose; last_pose = *it; it++; for (; it!=plan.end(); ++it) { path_length += hypot( (*it).pose.position.x - last_pose.pose.position.x, (*it).pose.position.y - last_pose.pose.position.y ); last_pose = *it; } cout <<"The global path length: "<< path_length<< " meters"<<endl; MyExcelFile << "\t" <<path_length <<"\t"<< plan.size() <<endl; return true; } else { ROS_WARN("The planner failed to find a path, choose other goal position"); return false; } } else { ROS_WARN("Not valid start or goal"); return false; } } void RAstarPlannerROS::getCorrdinate(float& x, float& y) { x = x - originX; y = y - originY; } int RAstarPlannerROS::convertToCellIndex(float x, float y) { int cellIndex; float newX = x / resolution; float newY = y / resolution; cellIndex = getCellIndex(newY, newX); return cellIndex; } void RAstarPlannerROS::convertToCoordinate(int index, float& x, float& y) { x = getCellColID(index) * resolution; y = getCellRowID(index) * resolution; x = x + originX; y = y + originY; } bool RAstarPlannerROS::isCellInsideMap(float x, float y) { bool valid = true; if (x > (width * resolution) || y > (height * resolution)) valid = false; return valid; } bool RAstarPlannerROS::init(int start_x, int start_y, int end_x, int end_y, vector<vector<int> >& pop) { int i