Skip to content

Commit

Permalink
Resources can be found in install tree based on libdrake.so location
Browse files Browse the repository at this point in the history
When an executable depends on `libdrake.so`, it can find the resources in the
install tree based on the location of `libdrake.so`. The implementation to
find `libdrake.so` is platform specific (MacOS and Linux). In both cases, it
finds the location of the library loaded in the process. It extracts its
directory and appends its with the relative path to the resource files.
This should limit the necessity of programmatically add path to the candidate
directory in which the sentinel file is searched.
  • Loading branch information
Francois Budin committed Dec 8, 2017
1 parent c84bceb commit 36b1bc1
Show file tree
Hide file tree
Showing 8 changed files with 214 additions and 36 deletions.
15 changes: 13 additions & 2 deletions common/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -233,11 +233,22 @@ DRAKE_RESOURCE_SENTINEL = adjust_label_for_drake_hoist(

drake_cc_library(
name = "find_resource",
srcs = ["find_resource.cc"],
hdrs = ["find_resource.h"],
srcs = [
"find_loaded_library.cc",
"find_resource.cc",
],
hdrs = [
"find_loaded_library.h",
"find_resource.h",
],
data = [
DRAKE_RESOURCE_SENTINEL,
],
# for libdrake.so path on linux
linkopts = select({
"//tools/cc_toolchain:linux": ["-ldl"],
"//conditions:default": [],
}),
deps = [
":essential",
"@spruce",
Expand Down
124 changes: 124 additions & 0 deletions common/find_loaded_library.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,124 @@
#include "drake/common/find_loaded_library.h"

#ifdef __APPLE__
#include <dlfcn.h>

#include <mach-o/dyld.h>
#include <mach-o/dyld_images.h>
#else // Not __APPLE__
#include <libgen.h>
#include <string.h>

#include <link.h>
#endif

using std::string;

namespace drake {

#ifdef __APPLE__

// This code has been adapted from:
// https://stackoverflow.com/questions/4309117/determining-programmatically-what-modules-are-loaded-in-another-process-os-x/23229148#23229148

// Reads memory from MacOS specific structures into an `unsigned char*`.
unsigned char * readProcessMemory(mach_vm_address_t addr,
mach_msg_type_number_t* size) {
mach_msg_type_number_t dataCnt =
reinterpret_cast<mach_msg_type_number_t>(*size);
vm_offset_t readMem;

kern_return_t kr = vm_read(mach_task_self(), addr, *size,
&readMem, &dataCnt);
if (kr) {
return NULL;
}
return ( reinterpret_cast<unsigned char *>(readMem));
}

// Gets the list of all the dynamic libraries that have been loaded. Finds
// `library_name` in the list, and returns its directory path appended
// with relative directory to find resource files in drake install tree.
// This function is specific to MacOS
optional<string> loaded_library_path(const std::string &library_name) {
optional<string> binary_dirname;
struct task_dyld_info dyld_info;
mach_msg_type_number_t count = TASK_DYLD_INFO_COUNT;
// Getinformation from current process.
if (task_info(mach_task_self(), TASK_DYLD_INFO,
reinterpret_cast<task_info_t>(&dyld_info), &count) == KERN_SUCCESS) {
// Recover list of dynamic libraries.
mach_msg_type_number_t size = sizeof(struct dyld_all_image_infos);
uint8_t* data =
readProcessMemory(dyld_info.all_image_info_addr, &size);
if (!data) {
return binary_dirname;
}
struct dyld_all_image_infos* infos =
reinterpret_cast<struct dyld_all_image_infos *>(data);

// Recover number of dynamic libraries in list.
mach_msg_type_number_t size2 =
sizeof(struct dyld_image_info) * infos->infoArrayCount;
uint8_t* info_addr = readProcessMemory(
reinterpret_cast<mach_vm_address_t>(infos->infoArray), &size2);
if (!info_addr) {
return binary_dirname;
}
struct dyld_image_info* info =
reinterpret_cast<struct dyld_image_info*>(info_addr);

// Loop over the dynamic libraries until `library_name` is found.
for (uint32_t i=0; i < infos->infoArrayCount; i++) {
const char * pos_slash = strrchr(info[i].imageFilePath, '/');
if (!strcmp(pos_slash + 1, library_name.c_str())) {
binary_dirname = string(info[i].imageFilePath,
pos_slash - info[i].imageFilePath);
break;
}
}
}
return binary_dirname;
}
#else // Not __APPLE__

// This code has been adapted from:
// http://syprog.blogspot.ru/2011/12/listing-loaded-shared-objects-in-linux.html

// Chained list of shared objects
struct lmap {
void* base_address; // Base address of the shared object
char* path; // Absolute file name (path) of the shared object
void* not_needed; // Pointer to the dynamic section of the SO
struct lmap *next, *prev; // chain of loaded objects
};

// Content of that dlopen is saved in this structure.
struct something {
void* pointers[3];
struct something* ptr;
};

// Gets the list of all the shared objects that have been loaded. Finds
// `library_name` in the list, and returns its directory path appended
// with relative directory to find resource files in drake install tree.
// This function is specific to Linux.
optional<string> loaded_library_path(const std::string &library_name) {
optional<string> binary_dirname;
struct lmap* pl;
void* ph = dlopen(NULL, RTLD_NOW);
struct something* p = reinterpret_cast<struct something*>(ph);
p = p->ptr;
pl = reinterpret_cast<struct lmap*>(p->ptr);
// Loop over loaded shared objects until `library_name` is found.
while (NULL != pl) {
if (!strcmp(basename(pl->path), library_name.c_str())) {
binary_dirname = string(dirname(pl->path));
break;
}
pl = pl->next;
}
return binary_dirname;
}
#endif
} // namespace drake
14 changes: 14 additions & 0 deletions common/find_loaded_library.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
#pragma once

#include <string>
#include <vector>

#include "drake/common/drake_optional.h"

namespace drake {


optional<std::string> loaded_library_path(const std::string &library_name);


} // namespace drake
16 changes: 14 additions & 2 deletions common/find_resource.cc
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include <spruce.hh>

#include "drake/common/drake_throw.h"
#include "drake/common/find_loaded_library.h"
#include "drake/common/never_destroyed.h"

using std::string;
Expand Down Expand Up @@ -106,6 +107,14 @@ optional<std::string> getenv_optional(const char* const name) {
return nullopt;
}

optional<std::string> resource_path_from_libdrake() {
optional<std::string> libdrake_dir = loaded_library_path("libdrake.so");
if (libdrake_dir) {
libdrake_dir = libdrake_dir.value() + "/../share/drake";
}
return libdrake_dir;
}

bool is_relative_path(const string& path) {
// TODO(jwnimmer-tri) Prevent .. escape?
return !path.empty() && (path[0] != '/');
Expand Down Expand Up @@ -217,8 +226,11 @@ Result FindResource(string resource_path) {
spruce::path candidate_dir(search_path);
candidate_dirs.emplace_back(check_candidate_dir(candidate_dir));
}

// (3) Search in cwd (and its parent, grandparent, etc.) to find Drake's
// (3) Find where `librake.so` is, and add search path that corresponds to
// resource folder in install tree based on `libdrake.so` location.
static optional<string> from_libdrake = resource_path_from_libdrake();
candidate_dirs.emplace_back(from_libdrake);
// (4) Search in cwd (and its parent, grandparent, etc.) to find Drake's
// resource-root sentinel file.
candidate_dirs.emplace_back(find_sentinel_dir());

Expand Down
37 changes: 5 additions & 32 deletions examples/kuka_iiwa_arm/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@ load(
"drake_cc_library",
"drake_cc_binary",
"drake_cc_googletest",
"drake_example_cc_binary",
)
load("//tools/install:install_data.bzl", "install", "install_data")
load("//tools/lint:lint.bzl", "add_lint_tests")
Expand Down Expand Up @@ -79,7 +80,7 @@ drake_cc_library(
],
)

drake_cc_binary(
drake_example_cc_binary(
name = "iiwa_controller",
srcs = ["iiwa_controller.cc"],
data = [
Expand All @@ -89,15 +90,11 @@ drake_cc_binary(
deps = [
":iiwa_common",
":lcm_plan_interpolator",
"//drake/common:text_logging_gflags",
"//drake/lcm",
"//drake/systems/lcm",
"//drake/systems/lcm:lcm_driven_loop",
"@com_github_gflags_gflags//:gflags",
],
)

drake_cc_binary(
drake_example_cc_binary(
name = "iiwa_wsg_simulation",
srcs = ["iiwa_wsg_simulation.cc"],
add_test_rule = 1,
Expand All @@ -113,25 +110,12 @@ drake_cc_binary(
":iiwa_common",
":iiwa_lcm",
":oracular_state_estimator",
"//drake/common:text_logging_gflags",
"//drake/examples/kuka_iiwa_arm/iiwa_world:iiwa_wsg_diagram_factory",
"//drake/lcm",
"//drake/manipulation/schunk_wsg:schunk_wsg_constants",
"//drake/manipulation/schunk_wsg:schunk_wsg_controller",
"//drake/manipulation/schunk_wsg:schunk_wsg_lcm",
"//drake/manipulation/util:world_sim_tree_builder",
"//drake/multibody/rigid_body_plant",
"//drake/systems/analysis",
"//drake/systems/controllers:inverse_dynamics_controller",
"//drake/systems/controllers:pid_controller",
"//drake/systems/primitives:constant_vector_source",
"//drake/systems/primitives:matrix_gain",
"//drake/util:lcm_util",
"@com_github_gflags_gflags//:gflags",
],
)

drake_cc_binary(
drake_example_cc_binary(
name = "kuka_simulation",
srcs = ["kuka_simulation.cc"],
add_test_rule = 1,
Expand All @@ -145,21 +129,11 @@ drake_cc_binary(
deps = [
":iiwa_common",
":iiwa_lcm",
"//drake/common:find_resource",
"//drake/common:text_logging_gflags",
"//drake/lcm",
"//drake/manipulation/util:sim_diagram_builder",
"//drake/manipulation/util:world_sim_tree_builder",
"//drake/multibody/rigid_body_plant",
"//drake/multibody/rigid_body_plant:frame_visualizer",
"//drake/systems/analysis:simulator",
"//drake/systems/controllers:inverse_dynamics_controller",
"//drake/systems/primitives:constant_vector_source",
"@com_github_gflags_gflags//:gflags",
],
)

drake_cc_binary(
drake_example_cc_binary(
name = "kuka_plan_runner",
srcs = ["kuka_plan_runner.cc"],
data = [
Expand All @@ -169,7 +143,6 @@ drake_cc_binary(
deps = [
":iiwa_common",
":iiwa_lcm",
"//drake/common:find_resource",
"@lcmtypes_bot2_core",
"@lcmtypes_robotlocomotion",
],
Expand Down
1 change: 1 addition & 0 deletions tools/drake.bzl
Original file line number Diff line number Diff line change
Expand Up @@ -8,4 +8,5 @@ load(
"drake_cc_googletest",
"drake_cc_library",
"drake_cc_test",
"drake_example_cc_binary",
)
1 change: 1 addition & 0 deletions tools/install/libdrake/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,7 @@ cc_library(
strip_include_prefix = "/" if HAS_MOVED_6996 else None,
visibility = adjust_labels_for_drake_hoist([
"//drake/bindings/pydrake:__subpackages__",
"//drake/examples:__subpackages__",
]),
deps = [
":gurobi_deps",
Expand Down
42 changes: 42 additions & 0 deletions tools/skylark/drake_cc.bzl
Original file line number Diff line number Diff line change
Expand Up @@ -513,3 +513,45 @@ def drake_cc_googletest(
name = name,
deps = deps,
**kwargs)

def drake_example_cc_binary(
name,
srcs = [],
data = [],
deps = [],
copts = [],
gcc_copts = [],
linkstatic = 1,
testonly = 0,
add_test_rule = 0,
test_rule_args = [],
test_rule_data = [],
test_rule_size = None,
test_rule_flaky = 0,
**kwargs):
"""Creates a rule to declare a C++ binary using `libdrake.so`.
This rule is a wrapper around `drake_cc_binary()`. It adds `libdrake.so`
and `drake_lcmtypes_headers` as dependencies to the target.
This allows the creation of examples for drake that depend on `libdrake.so`
which let the process discover the location of drake resources at runtime
based on the location of `libdrake.so` which is loaded by the process.
"""
drake_cc_binary(
name = name,
srcs = srcs +
["//tools/install/libdrake:libdrake.so",
"//lcmtypes:drake_lcmtypes_headers"],
data = data,
deps = deps + ["//tools/install/libdrake:drake_shared_library"],
copts = copts,
gcc_copts = gcc_copts,
linkstatic = linkstatic,
testonly = testonly,
add_test_rule = add_test_rule,
test_rule_args = test_rule_args,
test_rule_data = test_rule_data,
test_rule_size = test_rule_size,
test_rule_flaky = test_rule_flaky,
**kwargs)

0 comments on commit 36b1bc1

Please sign in to comment.