Method LoadStartEnd

src/plan_manager/src/plan_manager.cpp:359–422

}

void PlannerManager::LoadStartEnd()
{
  std::string fullPath = config.inputdata;
  std::string prefix = "shapes/";
  size_t startPos = fullPath.find(prefix);
  std::string shapeString;
  if (startPos != std::string::npos)
  {
    startPos += prefix.length();
    size_t endPos = fullPath.find(".obj", startPos);

    if (endPos != std::string::npos)
    {
      shapeString = fullPath.substr(startPos, endPos - startPos);
      std::cout << "ShapeString : " << shapeString << std::endl;
    }
    else
    {
      std::cout << "The string does not contain '.obj' after 'shapes/'" << std::endl;
    }
  }
  else
  {
    std::cout << "The string does not contain 'shapes/'" << std::endl;
  }

  cout << "\033[32m========load traj start and end points=======\033[0m" << endl;
  std::string filename = ros::package::getPath(string("plan_manager")) + "/pcds/trajectory_" + shapeString + ".txt";
  std::ifstream file(filename);
  std::string line;

  if (!file.is_open())
  {
    std::cerr << "无法打开文件 " << filename << std::endl;
  }
  std::string labelstart{"Start: "};

  while (std::getline(file, line))
  {
    if (line.find(labelstart) != std::string::npos)
    {
      std::istringstream iss(line.substr(labelstart.length()));
      iss >> Start_record.x() >> Start_record.y() >> Start_record.z();
      std::cout << std::setprecision(std::numeric_limits<double>::max_digits10);
      std::cout << "\033[32m Load start pose: " << Start_record.x() << ", " << Start_record.y() << ", " << Start_record.z() << "\033[0m" << std::endl;
      std::cout << std::setprecision(std::numeric_limits<double>::digits10);
      break;
    }
  }
  std::string labelend("End: ");
  while (std::getline(file, line))
  {
    if (line.find(labelend) != std::string::npos)
    {
      std::istringstream iss(line.substr(labelend.length()));
      iss >> End_record.x() >> End_record.y() >> End_record.z();
      std::cout << std::setprecision(std::numeric_limits<double>::max_digits10);
      std::cout << "\033[32m Load End pose: " << End_record.x() << ", " << End_record.y() << ", " << End_record.z() << "\033[0m" << std::endl;