Пример #1
0
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;
}
Пример #2
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;
}