Visualize Hydroelastic and Point Contacts (continued from #25)

bazel_ros2_rules/ros2/ament_index.bzl

@@ -0,0 +1,107 @@
+# -*- python -*-
+
+"""
+Provides bazel rules for working with an ament resource index.
+
+See this for more information:
+https://github.com/ament/ament_cmake/
+blob/master/ament_cmake_core/doc/resource_index.md
+"""
+
+load("@bazel_skylib//lib:paths.bzl", "paths")
+load("//tools:ament_index.bzl", "AmentIndex")
+
+def _ament_index_share_files_impl(ctx):
+    # declare that a "package" with the given name exists
+    package_marker_path = paths.join(
+        ctx.attr.prefix,
+        "share/ament_index/resource_index/packages/",
+        ctx.attr.package_name,
+    )
+    package_marker_out = ctx.actions.declare_file(package_marker_path)
+    ctx.actions.write(
+        output = package_marker_out,
+        content = "",
+    )
+
+    # Symlink sources into the share directory
+    runfiles_symlinks = {
+        package_marker_path: package_marker_out,
+    }
+
+    for src in ctx.attr.srcs:
+        # src is a target that could have multiple files
+        for file in src.files.to_list():
+            sp = file.short_path
+            if sp.startswith(ctx.attr.strip_prefix):
+                sp = sp[len(ctx.attr.strip_prefix):]
+            symlink_path = paths.join(
+                ctx.attr.prefix,
+                "share",
+                ctx.attr.package_name,
+                sp,
+            )
+            runfiles_symlinks[symlink_path] = file
+
+    return [
+        AmentIndex(prefix = ctx.attr.prefix),
+        DefaultInfo(
+            runfiles = ctx.runfiles(root_symlinks = runfiles_symlinks),
+        ),
+    ]
+
+ament_index_share_files = rule(
+    attrs = dict(
+        package_name = attr.string(mandatory = True),
+        srcs = attr.label_list(
+            mandatory = True,
+            allow_empty = False,
+            allow_files = True,
+        ),
+        # A prefix is required because the shim can't prepend the runfiles
+        # root to AMENT_PREFIX_PATH
+        prefix = attr.string(default = "ament_index_share_files"),
+        strip_prefix = attr.string(default = ""),
+    ),
+    implementation = _ament_index_share_files_impl,
+    output_to_genfiles = True,
+    provides = [AmentIndex],
+)
+"""
+Creates an ament resource index and share/ directory for a single package.
+
+This creates both a package marker for a package, and a share directory
+for that package.
+Files in `srcs` will be added to `share/package_name`.
+A target depending on this one will be able to access the files in the
+share directory using the package's share directory joined with the
+files they expect.
+
+    d = ament_index_cpp::get_package_share_directory("package_name")
+    file = join(d, "short_path of file")
+
+Note that this works only when bazel creates runfiles links,
+and not just a *.runfiles_manifest (which it might do on an unsupported
+platform like Windows, or with `--nobuild_runfile_links`).
+
+Note that the same package_name should never be used in two different
+calls to `ament_index_share_files` given to the same target.
+This is called "overriding", and it's not currently supported.
+Currently if the two calls to `ament_index_share_files` also have
+the same value for "prefix", then a target depending on both may
+be able to find resources from both rules, but that behavior is not
+guaranteed.
+TODO(sloretz) detect and error when a target is given two ament indexes
+with the same package.
+
+Args:
+    package_name: name of a ROS 2 package to which these share files belong
+    srcs: files to put into the share directory
+    prefix: optional prefix to give to the generated runfiles.
+    strip_prefix: optional prefix to strip from the short_path of the files
+
+Provides:
+    AmentIndex: a prefix path where the ament resource index was generated.
+    DefaultInfo: folders and symlinked files to add to the runfiles of a
+      dependent target.
+"""

bazel_ros2_rules/ros2/defs.bzl

@@ -4,6 +4,7 @@ load("@bazel_tools//tools/build_defs/repo:utils.bzl", "patch", "update_attrs")
 load("//tools:execute.bzl", "execute_or_fail")
 
 COMMON_FILES_MANIFEST = [
+    "ament_index.bzl",
     "ros_cc.bzl",
     "ros_py.bzl",
     "rosidl.bzl",

bazel_ros2_rules/ros2/rosidl.bzl

@@ -266,7 +266,9 @@ rosidl_generate_ament_index_entry = rule(
             allow_empty = False,
             allow_files = True,
         ),
-        prefix = attr.string(default = "."),
+        # A prefix is required because the shim can't prepend the runfiles
+        # root to AMENT_PREFIX_PATH
+        prefix = attr.string(default = "rosidl_generate_ament_index_entry"),
     ),
     implementation = _rosidl_generate_ament_index_entry_impl,
     output_to_genfiles = True,

bazel_ros2_rules/ros2/tools/dload_shim.cc

@@ -77,6 +77,9 @@ void ApplyEnvironmentActions(
           std::abort();
         }
         for (size_t j = 1; j < actions[i].size(); ++j) {
+          // Not seeing explicitly set values in your environment variable?
+          // Rlocation returns an empty string if the path cannot be found,
+          // or contains "./", "../", etc.
           value_stream << runfiles->Rlocation(actions[i][j]) << ":";
         }
 

drake_ros/CMakeLists.txt

@@ -77,7 +77,9 @@ ament_export_dependencies(geometry_msgs)
 ament_export_dependencies(rclcpp)
 ament_export_dependencies(rosidl_runtime_c)
 ament_export_dependencies(rosidl_typesupport_cpp)
+ament_export_dependencies(tf2_eigen)
 ament_export_dependencies(tf2_ros)
 ament_export_dependencies(visualization_msgs)
+ament_export_dependencies(VTK)
 
 ament_package()

drake_ros/package.xml

@@ -17,11 +17,13 @@
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
   <build_depend>eigen</build_depend>
 
+  <depend>libvtk9-dev</depend>
   <depend>rclcpp</depend>
   <depend>rclpy</depend>
   <depend>rosidl_runtime_c</depend>
   <depend>rosidl_typesupport_cpp</depend>
   <depend>geometry_msgs</depend>
+  <depend>tf2_eigen</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_clang_format</test_depend>

drake_ros/required_packages.bzl

@@ -4,6 +4,7 @@ DRAKE_ROS_REQUIRED_PACKAGES = [
     "rclcpp",
     "rosidl_runtime_c",
     "rosidl_typesupport_cpp",
+    "tf2_eigen",
     "tf2_ros",
     "visualization_msgs",
 ]

drake_ros/viz/BUILD.bazel

@@ -9,7 +9,9 @@ cc_library(
         "//tf2:odr_safe_deps",
         "@ros2//:geometry_msgs_cc",
         "@ros2//:rclcpp_cc",
+        "@ros2//:tf2_eigen_cc",
         "@ros2//:visualization_msgs_cc",
+        "@vtk//:vtkIOImage",
     ],
 )
 

drake_ros/viz/CMakeLists.txt

@@ -1,8 +1,12 @@
 find_package(geometry_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(tf2_eigen REQUIRED)
 find_package(visualization_msgs REQUIRED)
+find_package(VTK REQUIRED)
 
 set(HEADERS
+  "contact_markers_system.h"
+  "defaults.h"
   "name_conventions.h"
   "rviz_visualizer.h"
   "scene_markers_system.h"
@@ -19,16 +23,20 @@ add_library(drake_ros_viz
   name_conventions.cc
   rviz_visualizer.cc
   scene_markers_system.cc
+  contact_markers_system.cc
 )
 
 target_link_libraries(drake_ros_viz PUBLIC
     drake::drake
     drake_ros_core
     drake_ros_tf2
     rclcpp::rclcpp
+    tf2_eigen::tf2_eigen
     ${geometry_msgs_TARGETS}
     ${visualization_msgs_TARGETS}
 )
+target_link_libraries(drake_ros_viz PRIVATE
+  VTK::IOImage)
 
 target_include_directories(drake_ros_viz
   PUBLIC