file-explorer.cpp 2.06 KB
Newer Older
1
//
Justin Carpentier's avatar
Justin Carpentier committed
2
// Copyright (c) 2016-2021 CNRS INRIA
3
4
//

Justin Carpentier's avatar
Justin Carpentier committed
5
#include <cstdlib>
6
#include <boost/filesystem.hpp>
7
8
#include "pinocchio/utils/file-explorer.hpp"

9
10
11
12
13
14
#ifdef _WIN32
const char delimiter = ';';
#else
const char delimiter = ':';
#endif

15
namespace fs = boost::filesystem;
16
17
18
19
namespace pinocchio
{

  void extractPathFromEnvVar(const std::string & env_var_name,
20
                             std::vector<std::string> & list_of_paths)
21
22
23
24
25
26
27
  {
    const char * env_var_value = std::getenv(env_var_name.c_str());
    
    if(env_var_value != NULL)
    {
      std::string policyStr(env_var_value);
      // Add a separator at the end so that last path is also retrieved
28
      policyStr += delimiter;
29
30
31
32
      size_t lastOffset = 0;
      while(true)
      {
        size_t offset = policyStr.find_first_of(delimiter, lastOffset);
33
34
        if (offset < policyStr.size())
          list_of_paths.push_back(policyStr.substr(lastOffset, offset - lastOffset));
35
36
37
38
39
40
41
42
        if (offset == std::string::npos)
          break;
        else
          lastOffset = offset + 1; // add one to skip the delimiter
      }
    }
  }

43
  std::vector<std::string> extractPathFromEnvVar(const std::string & env_var_name)
44
45
  {
    std::vector<std::string> list_of_paths;
46
    extractPathFromEnvVar(env_var_name,list_of_paths);
47
48
49
50
51
    return list_of_paths;
  }

  std::vector<std::string> rosPaths()
  {
52
53
54
55
56
57
58
    std::vector<std::string> raw_list_of_paths;
    extractPathFromEnvVar("ROS_PACKAGE_PATH", raw_list_of_paths);
    extractPathFromEnvVar("AMENT_PREFIX_PATH", raw_list_of_paths);

    // Work-around for https://github.com/stack-of-tasks/pinocchio/issues/1463
    // To support ROS devel/isolated spaces, we also need to look one package above the package.xml:
    fs::path path;
59
    std::vector<std::string> list_of_paths;
Wolfgang Merkt's avatar
Wolfgang Merkt committed
60
61
62
    for (std::vector<std::string>::iterator it = raw_list_of_paths.begin(); it != raw_list_of_paths.end(); ++it) {
      list_of_paths.push_back(*it);
      path = fs::path(*it);
63
64
65
66
      if (fs::exists(path / "package.xml")) {
        list_of_paths.push_back(fs::path(path / "..").string());
      }
    }
67
68
69
70
    return list_of_paths;
  }

}