int main(int argc, char** argv) { //load map readMap(); num_expands = 0; num_discovered = 1; //run planner test_duration = 0; Clock::time_point t0 = Clock::now(); timeout = t0 + chrono::seconds(5); prepare(); search(); Clock::time_point t1 = Clock::now(); //report joint plan testSolution(joint_solution, true); //report stats double dt = chrono::duration<double, chrono::seconds::period>(t1-t0).count(); cout << "results: Path Length=" << joint_cost/10000.0; cout << " Visited Nodes=" << num_discovered; cout << " Explored Nodes=" << num_expands; cout << " Planning Time=" << dt << endl; cout << " Joint Testing Time=" << test_duration << endl; FILE* fout = fopen("stats.csv","w"); fprintf(fout,"%d %f %d %f %f\n",W,joint_cost/10000.0,num_expands,dt,test_duration); fclose(fout); for (int a = 0; a < agents.size(); ++a) { fout = fopen("visualf.txt","w"); vector<string> visual = visualizePath(); for (int i = 0; i < visual.size(); ++i) fprintf(fout,"%s\n",visual[i].c_str()); fclose(fout); } }
void OpenPlannerSimulator::PlannerMainLoop() { ros::Rate loop_rate(100); //PlannerHNS::WayPoint defaultStart(3704.15014648,-99459.0743942, 88, 3.12940141294); PlannerHNS::BehaviorState currBehavior; PlannerHNS::VehicleState currStatus; PlannerHNS::VehicleState desiredStatus; while (ros::ok()) { ros::spinOnce(); bool bMakeNewPlan = false; if(m_SimParams.mapSource == MAP_KML_FILE && !m_bMap) { m_bMap = true; PlannerHNS::MappingHelpers::LoadKML(m_SimParams.KmlMapPath, m_Map); if(!m_SimParams.bRandomStart) InitializeSimuCar(m_SimParams.startPose); } else if (m_SimParams.mapSource == MAP_FOLDER && !m_bMap) { m_bMap = true; PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_SimParams.KmlMapPath, m_Map, true); if(!m_SimParams.bRandomStart) InitializeSimuCar(m_SimParams.startPose); } if(m_bMap && bInitPos) { double dt = UtilityHNS::UtilityH::GetTimeDiffNow(m_PlanningTimer); UtilityHNS::UtilityH::GetTickCount(m_PlanningTimer); //Global Planning Step if(m_LocalPlanner.m_TotalPath.size() > 0 && m_LocalPlanner.m_TotalPath.at(0).size() > 3) { PlannerHNS::RelativeInfo info; bool ret = PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_LocalPlanner.m_TotalPath, m_LocalPlanner.state, 0.75, info); if(ret == true && info.iGlobalPath >= 0 && info.iGlobalPath < m_LocalPlanner.m_TotalPath.size() && info.iFront > 0 && info.iFront < m_LocalPlanner.m_TotalPath.at(info.iGlobalPath).size()) { double remaining_distance = m_LocalPlanner.m_TotalPath.at(info.iGlobalPath).at(m_LocalPlanner.m_TotalPath.at(info.iGlobalPath).size()-1).cost - (m_LocalPlanner.m_TotalPath.at(info.iGlobalPath).at(info.iFront).cost + info.to_front_distance); if(remaining_distance <= REPLANNING_DISTANCE) { bMakeNewPlan = true; if(m_SimParams.bLooper) InitializeSimuCar(m_SimParams.startPose); } } } else bMakeNewPlan = true; if(bMakeNewPlan) { std::vector<std::vector<PlannerHNS::WayPoint> > generatedTotalPaths; double ret = m_GlobalPlanner.PlanUsingDPRandom(m_LocalPlanner.state, PLANNING_DISTANCE, m_Map, generatedTotalPaths); for(unsigned int i=0; i < generatedTotalPaths.size(); i++) { PlannerHNS::PlanningHelpers::CalcAngleAndCost(generatedTotalPaths.at(i)); } m_LocalPlanner.m_TotalPath = generatedTotalPaths; m_LocalPlanner.m_pCurrentBehaviorState->GetCalcParams()->bNewGlobalPath = true; } //Local Planning std::vector<TrafficLight> trafficLight; currBehavior = m_LocalPlanner.DoOneStep(dt, currStatus, m_TrackedClusters, 1, m_Map, 0, trafficLight, true); //Odometry Simulation and Update m_LocalPlanner.SetSimulatedTargetOdometryReadings(desiredStatus.speed, desiredStatus.steer, desiredStatus.shift); m_LocalPlanner.UpdateState(desiredStatus, false); m_LocalPlanner.LocalizeMe(dt); currStatus.shift = desiredStatus.shift; currStatus.steer = m_LocalPlanner.m_CurrentSteering; currStatus.speed = m_LocalPlanner.m_CurrentVelocity; //Path Following and Control desiredStatus = m_PredControl.DoOneStep(dt, currBehavior, m_LocalPlanner.m_Path, m_LocalPlanner.state, currStatus, currBehavior.bNewPlan); displayFollowingInfo(m_LocalPlanner.m_TrajectoryCostsCalculatotor.m_SafetyBorder.points, m_LocalPlanner.state); visualizePath(m_LocalPlanner.m_Path); visualizeBehaviors(); geometry_msgs::PoseArray sim_data; geometry_msgs::Pose p_id, p_pose, p_box; sim_data.header.frame_id = "map"; sim_data.header.stamp = ros::Time(); p_id.position.x = m_SimParams.id; PlannerHNS::WayPoint pose_center = GetRealCenter(m_LocalPlanner.state); p_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, UtilityHNS::UtilityH::SplitPositiveAngle(pose_center.pos.a)); p_pose.position.x = pose_center.pos.x; p_pose.position.y = pose_center.pos.y; p_pose.position.z = pose_center.pos.z; p_box.position.x = m_CarInfo.width; p_box.position.y = m_CarInfo.length; p_box.position.z = 2.2; sim_data.poses.push_back(p_id); sim_data.poses.push_back(p_pose); sim_data.poses.push_back(p_box); pub_SimuBoxPose.publish(sim_data); if(currBehavior.bNewPlan) { std::ostringstream str_out; str_out << m_SimParams.logPath; str_out << "LocalPath_"; PlannerHNS::PlanningHelpers::WritePathToFile(str_out.str(), m_LocalPlanner.m_Path); } if(m_SimParams.bLooper && currBehavior.state == PlannerHNS::FINISH_STATE) { InitializeSimuCar(m_SimParams.startPose); } } loop_rate.sleep(); } }