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();
}
}
