int main(int argc, char** argv) {
srand( time(0));
ros::init(argc, argv, "ramp_planner");
ros::NodeHandle handle;
// Load ros parameters and obstacle transforms
//loadParameters(handle);
//loadObstacleTF();
num_obs = 3;
ros::Rate r(100);
/*
* Data to collect
*/
std::vector<bool> reached_goal;
std::vector<bool> bestTrajec_fe;
std::vector<double> time_left;
std::vector<bool> stuck_in_ic;
std::vector<bool> ic_occurred;
std::vector<TestCaseTwo> test_cases;
ros::Timer ob_trj_timer;
ob_trj_timer.stop();
int num_tests = 42;
ob_delay.push_back(2);
ob_delay.push_back(2);
ob_delay.push_back(4);
// Make an ObstacleList Publisher
pub_obs = handle.advertise<ramp_msgs::ObstacleList>("obstacles", 1);
ros::ServiceClient trj_gen = handle.serviceClient<ramp_msgs::TrajectorySrv>("trajectory_generator");
ros::Subscriber sub_bestT = handle.subscribe("bestTrajec", 1, bestTrajCb);
ros::Subscriber sub_imminent_collision_ = handle.subscribe("imminent_collision", 1, imminentCollisionCb);
ros::Subscriber sub_update = handle.subscribe("update", 1, updateCb);
std::ofstream f_reached;
f_reached.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/reached_goal.txt",
std::ios::out | std::ios::app | std::ios::binary);
std::ofstream f_feasible;
f_feasible.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/feasible.txt", std::ios::out
| std::ios::app | std::ios::binary);
std::ofstream f_time_left;
f_time_left.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/time_left.txt",
std::ios::out | std::ios::app | std::ios::binary);
std::ofstream f_ic_stuck;
f_ic_stuck.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/ic_stuck.txt", std::ios::out
| std::ios::app | std::ios::binary);
std::ofstream f_ic_occurred;
f_ic_occurred.open("/home/sterlingm/ros_workspace/src/ramp/ramp_planner/system_level_data/8/ic_occurred.txt",
std::ios::out | std::ios::app | std::ios::binary);
// Set flag signifying that the next test case is not ready
ros::param::set("/ramp/tc_generated", false);
ros::Duration d_history(1);
ros::Duration d_test_case_thresh(20);
for(int i=0;i<num_tests;i++)
{
MotionState initial_state;
//my_planner.randomMS(initial_state);
/*
*
* Generate a test case
*
*/
/*
* Generate the test case
*/
ABTC abtc;
/*
* Create test case where all obstacles stop, move, stop for 1 second each
*/
for(int i_ob=0;i_ob<3;i_ob++)
{
abtc.moving[i_ob] = 0;
abtc.moving[i_ob+3] = 1;
abtc.moving[i_ob+6] = 0;
abtc.times[i_ob] = 1;
abtc.times[i_ob+3] = 1;
abtc.times[i_ob+6] = 1;
}
generateObInfoGrid(initial_state);
/*
* Get test data for the abtc
*/
TestCaseTwo tc = generateTestCase(initial_state, num_obs);
tc.abtc = abtc;
/*
* Get trajectories for each obstacle
*/
ramp_msgs::TrajectorySrv tr_srv;
for(int i=0;i<tc.obs.size();i++)
{
ramp_msgs::TrajectoryRequest tr;
tf::Transform tf;
tf.setOrigin( tf::Vector3(0,0,0) );
tf.setRotation( tf::createQuaternionFromYaw(0) );
MotionType mt = my_planner.findMotionType(tc.obs[i].msg);
ramp_msgs::Path p = my_planner.getObstaclePath(tc.obs[i].msg, mt);
tr.path = p;
tr.type = PREDICTION;
tr_srv.request.reqs.push_back(tr);
}
// Call trajectory generator
if(trj_gen.call(tr_srv))
{
for(int i=0;i<tr_srv.response.resps.size();i++)
{
ROS_INFO("Traj: %s", utility.toString(tr_srv.response.resps[i].trajectory).c_str());
tc.ob_trjs.push_back(tr_srv.response.resps[i].trajectory);
}
}
else
{
ROS_ERROR("Error in getting obstacle trajectories");
}
/*
* Get static version of obstacles
*/
ramp_msgs::ObstacleList obs_stat;
for(int i=0;i<tc.obs.size();i++)
{
obs_stat.obstacles.push_back(getStaticOb(tc.obs[i].msg));
}
ROS_INFO("Generate: obs_stat.size(): %i", (int)obs_stat.obstacles.size());
IC_occur = false;
// Set flag signifying that the next test case is ready
ros::param::set("/ramp/tc_generated", true);
ROS_INFO("Generate: Waiting for planner to prepare");
/*
* Wait for planner to be ready to start test case
*/
bool start_tc = false;
while(!start_tc)
{
handle.getParam("/ramp/ready_tc", start_tc);
r.sleep();
ros::spinOnce();
}
ROS_INFO("Generate: Planner ready, publishing static obstacles");
// Publish static obstacles
pub_obs.publish(obs_stat);
// Wait for 1 second
d_history.sleep();
ROS_INFO("Generate: Done sleeping for 1 second");
// Publish dynamic obstacles
//pub_obs.publish(tc.ob_list);
tc.t_begin = ros::Time::now();
// Create timer to continuously publish obstacle information
ob_trj_timer = handle.createTimer(ros::Duration(1./20.), boost::bind(pubObTrj, _1, tc));
/*
* Wait for planner to run for time threshold
*/
while(start_tc)
{
handle.getParam("ramp/ready_tc", start_tc);
//ROS_INFO("generate_test_case: start_tc: %s", start_tc ? "True" : "False");
r.sleep();
ros::spinOnce();
}
ros::Duration elasped = ros::Time::now() - tc.t_begin;
ROS_INFO("generate_test_case: Test case completed");
// Set flag signifying that the next test case is not ready
ros::param::set("/ramp/tc_generated", false);
ob_trj_timer.stop();
/*
* Collect data
*/
MotionState goal;
goal.msg_.positions.push_back(2);
goal.msg_.positions.push_back(2);
goal.msg_.positions.push_back(PI/4.f);
double dist = utility.positionDistance( goal.msg_.positions, latestUpdate.msg_.positions );
//if(elasped.toSec()+0.01 < d_test_case_thresh.toSec())
//if(bestTrajec.trajectory.points.size() < 3)
if(dist < 0.2)
{
reached_goal.push_back(true);
f_reached<<true<<std::endl;
}
else
{
reached_goal.push_back(false);
f_reached<<false<<std::endl;
}
if(bestTrajec.feasible)
{
bestTrajec_fe.push_back(true);
time_left.push_back( bestTrajec.trajectory.points[ bestTrajec.trajectory.points.size()-1
].time_from_start.toSec());
f_feasible<<true<<std::endl;
f_time_left<<bestTrajec.trajectory.points[ bestTrajec.trajectory.points.size()-1
].time_from_start.toSec()<<std::endl;
}
else
{
bestTrajec_fe.push_back(false);
f_feasible<<false<<std::endl;
//time_left.push_back(9999);
}
if(IC_current)
{
stuck_in_ic.push_back(true);
f_ic_stuck<<true<<std::endl;
}
else
{
stuck_in_ic.push_back(false);
f_ic_stuck<<false<<std::endl;
}
if(IC_occur)
{
ic_occurred.push_back(true);
f_ic_occurred<<true<<std::endl;
}
else
{
ic_occurred.push_back(false);
f_ic_occurred<<false<<std::endl;
}
bestTrajec_at_end.push_back(bestTrajec);
test_cases.push_back(tc);
} // end for
f_reached.close();
f_feasible.close();
f_time_left.close();
f_ic_stuck.close();
f_ic_occurred.close();
std::cout<<"\n\nExiting Normally\n";
ros::shutdown();
return 0;
}
/*
* Does NOT generate an ABTC
* Put that in later on once single test case is working well
*/
TestCaseTwo generateTestCase(const MotionState robot_state, int num_obs)
{
TestCaseTwo result;
ROS_INFO("In generateTestCase");
ROS_INFO("num_obs: %i", num_obs);
// Generate all obstacles and push them onto test case
for(int i=0;i<num_obs;i++)
{
ObInfo temp = generateObInfoGrid(robot_state);
if(i == 1)
{
// Get position distance from other obstacle
std::vector<double> one, two;
one.push_back(result.obs[0].x);
one.push_back(result.obs[0].y);
two.push_back(temp.x);
two.push_back(temp.y);
double dist = utility.positionDistance(one, two);
while(dist < 0.2) {
ROS_INFO("one: (%f, %f) temp: (%f, %f)", one[0], one[1], two[0], two[1]);
temp = generateObInfoGrid(robot_state);
two[0] = temp.x;
two[1] = temp.y;
dist = utility.positionDistance(one, two);
}
}
else if (i == 2)
{
// Get position distance from other two obstacles
std::vector<double> one, two, three;
one.push_back(result.obs[0].x);
one.push_back(result.obs[0].y);
two.push_back(result.obs[1].x);
two.push_back(result.obs[1].y);
three.push_back(temp.x);
three.push_back(temp.y);
double dist1 = utility.positionDistance(one, three);
double dist2 = utility.positionDistance(two, three);
while(dist1 < 0.2 || dist2 < 0.2)
{
ROS_INFO("one: (%f, %f) two:(%f, %f) temp: (%f, %f)", one[0], one[1], two[0], two[1], three[0], three[1]);
temp = generateObInfoGrid(robot_state);
three[0] = temp.x;
three[1] = temp.y;
dist1 = utility.positionDistance(one, three);
dist2 = utility.positionDistance(two, three);
}
}
temp.msg = buildObstacleMsg(temp.x, temp.y, temp.v, temp.relative_direction, temp.w);
result.obs.push_back(temp);
result.ob_list.obstacles.push_back(temp.msg);
ROS_INFO("result.obs.size(): %i", (int)result.obs.size());
ROS_INFO("result.ob_list.obstacles.size(): %i", (int)result.ob_list.obstacles.size());
}
return result;
}