From ef17e8fd437fa6c776c0066f833e427a5a00ea5c Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Wed, 17 Apr 2024 20:23:30 -0400 Subject: [PATCH] address review --- bazel_ros2_rules/ros2/BUILD.bazel | 11 --- bazel_ros2_rules/ros2/README.md | 21 +++- .../resources/bazel_ros_env/bazel_ros_env.py | 5 +- bazel_ros2_rules/ros2/ros_py.bzl | 96 +++++++++++++++++-- .../ros2/tools/roslaunch_util/__init__.py | 3 - .../tools/roslaunch_util/roslaunch_util.py | 52 ---------- .../ros2/tools/roslaunch_util/setup.py | 10 -- .../ros2_example_apps/BUILD.bazel | 37 +++++-- .../ros2_example_apps/eg_launch.py | 14 ++- .../ros2_example_apps/eg_launch.xml | 8 +- .../roslaunch_eg_nodes/eg_listener.cpp | 27 +++++- .../roslaunch_eg_nodes/eg_talker.py | 45 +++++---- .../test/roslaunch_eg_test.py | 30 ++++++ 13 files changed, 234 insertions(+), 125 deletions(-) delete mode 100644 bazel_ros2_rules/ros2/tools/roslaunch_util/__init__.py delete mode 100644 bazel_ros2_rules/ros2/tools/roslaunch_util/roslaunch_util.py delete mode 100644 bazel_ros2_rules/ros2/tools/roslaunch_util/setup.py create mode 100644 ros2_example_bazel_installed/ros2_example_apps/test/roslaunch_eg_test.py diff --git a/bazel_ros2_rules/ros2/BUILD.bazel b/bazel_ros2_rules/ros2/BUILD.bazel index 354a7bdf..c6697127 100644 --- a/bazel_ros2_rules/ros2/BUILD.bazel +++ b/bazel_ros2_rules/ros2/BUILD.bazel @@ -22,17 +22,6 @@ py_library( deps = ["@bazel_tools//tools/python/runfiles"], ) -py_library( - name = "roslaunch_util.py", - srcs = [ - "tools/roslaunch_util/__init__.py", - "tools/roslaunch_util/roslaunch_util.py", - "tools/roslaunch_util/setup.py", - ], - visibility = ["//visibility:public"], - deps = ["@bazel_tools//tools/python/runfiles"], -) - run_calculate_rosidl_capitalization_tests() py_library( diff --git a/bazel_ros2_rules/ros2/README.md b/bazel_ros2_rules/ros2/README.md index bf71c139..3618483a 100644 --- a/bazel_ros2_rules/ros2/README.md +++ b/bazel_ros2_rules/ros2/README.md @@ -91,6 +91,10 @@ equivalent to the native `py_binary` and `py_test` rules, ensure these binaries run in an environment that is tightly coupled with the underlying ROS 2 workspace install space. +To generate a Python or XML launch target, `ros_launch` is available in the +`ros_py.bzl` file. Note that there are some nuances; pelase see the Launch +Files section below. + To generate and build ROS 2 interfaces, a `rosidl_interfaces_group` rule is available in the `rosidl.bzl` file. This rule generates C++ and Python code for the given ROS 2 interface definition files and builds them using what is @@ -103,6 +107,21 @@ the same file. By default, these naming conventions allow downstream `rosidl_interfaces_group` rules to depend on upstream `rosidl_interface_group` rules. +#### Launch Files + +An example of the `ros_launch` macro can be found under `ros2_example_apps`. +Please note the following limitations: + +- Exposing a Bazel package as a ROS package has not yet been done. Once that is + done, `launch_ros.actions.Node` / `` can be used. +- For Python launch files, it is best to use `Rlocation` (as shown in the example) + so that the launch file can be run via `bazel run`, `bazel test`, and + `./bazel-bin` (directly). +- For XML launch files, we need to (somehow) expose either ROS packages or + `Rlocation`. This needs to be done in a way that can be discovered by + [`Parser.load_launch_extensions`](https://github.com/ros2/launch/blob/698e979382877242621a0d633750fe96ff0c2bca/launch/launch/frontend/parser.py#L72-L87), + which may require care to do so correctly in Bazel. + ### Tools The `rmw_isolation` subpackage provides C++ and Python `isolate_rmw_by_path` @@ -188,9 +207,9 @@ From the above two examples (at present), the following features are in directly, but instead leverage the C-level interfaces, we get into an awkward ground of mutually exclusive memory / resource management paradigms. +- Affordances for `ros2 launch` The other repos, however, have the following that `bazel_ros2_rules` is missing: -- Affordances for `ros2 launch` - Launching from containers (Docker, Apptainer) diff --git a/bazel_ros2_rules/ros2/resources/bazel_ros_env/bazel_ros_env.py b/bazel_ros2_rules/ros2/resources/bazel_ros_env/bazel_ros_env.py index d5c73935..41d1f6ca 100644 --- a/bazel_ros2_rules/ros2/resources/bazel_ros_env/bazel_ros_env.py +++ b/bazel_ros2_rules/ros2/resources/bazel_ros_env/bazel_ros_env.py @@ -64,7 +64,10 @@ def _runfiles(): def Rlocation(path): - return _runfiles().Rlocation(path) + result = _runfiles().Rlocation(path) + if result is None: + raise RuntimeError(f"Unable to find Rlocation({repr(path)})") + return result def make_bazel_runfiles_env(): diff --git a/bazel_ros2_rules/ros2/ros_py.bzl b/bazel_ros2_rules/ros2/ros_py.bzl index 5048298d..c20c1802 100644 --- a/bazel_ros2_rules/ros2/ros_py.bzl +++ b/bazel_ros2_rules/ros2/ros_py.bzl @@ -130,24 +130,100 @@ def ros_py_binary( ) py_binary_rule(name = name, **kwargs) +def _add_deps(existing, new): + deps = list(existing) + for dep in new: + if dep not in deps: + deps.append(dep) + return deps + +def _generate_file_impl(ctx): + out = ctx.actions.declare_file(ctx.label.name) + ctx.actions.write(out, ctx.attr.content, ctx.attr.is_executable) + return [DefaultInfo( + files = depset([out]), + data_runfiles = ctx.runfiles(files = [out]), + )] + +_generate_file = rule( + attrs = { + "content": attr.string(mandatory = True), + "is_executable": attr.bool(default = False), + }, + output_to_genfiles = True, + implementation = _generate_file_impl, +) + +_LAUNCH_PY_TEMPLATE = """ +import os +import sys + +from bazel_ros_env import Rlocation + +assert __name__ == "__main__" +launch_file = Rlocation({launch_respath}) +ros2_bin = Rlocation("ros2/ros2") +args = [ros2_bin, "launch", launch_file] + sys.argv[1:] +os.execv(ros2_bin, args) +""" + +def _make_respath(relpath, workspace_name): + repo = native.repository_name() + if repo == "@": + if workspace_name == None: + fail( + "Please provide `ros_launch(*, workspace_name)` so that " + + "paths can be resolved properly", + ) + repo = workspace_name + pkg = native.package_name() + if pkg != "": + pieces = [repo, pkg, relpath] + else: + pieces = [repo, relpath] + return "/".join(pieces) + def ros_launch( name, - launch_file = None, - node_targets = []): - deps = ["@ros2//:ros2", "@bazel_ros2_rules//ros2:roslaunch_util.py"] - srcs = ["@bazel_ros2_rules//ros2:roslaunch_util.py"] + launch_file, + args = [], + data = [], + deps = [], + visibility = None, + # TODO(eric.cousineau): Remove this once Bazel provides a way to tell + # runfiles.py to use "this repository" in a way that doesn't require + # bespoke information. + workspace_name = None, + **kwargs): + main = "{}_roslaunch_main.py".format(name) + launch_respath = _make_respath(launch_file, workspace_name) - data = [launch_file] - data += node_targets - args = [launch_file] + content = _LAUNCH_PY_TEMPLATE.format( + launch_respath = repr(launch_respath), + ) + _generate_file( + name = main, + content = content, + visibility = ["//visibility:private"], + ) + + deps = _add_deps( + deps, + [ + "@ros2//:ros2", + "@ros2//resources/bazel_ros_env:bazel_ros_env_py", + ], + ) + data = data + [launch_file] ros_py_binary( name = name, - main = "@bazel_ros2_rules//ros2:roslaunch_util.py", + main = main, deps = deps, - srcs = srcs, + srcs = [main], data = data, - args = args, + visibility = visibility, + **kwargs ) def ros_py_test( diff --git a/bazel_ros2_rules/ros2/tools/roslaunch_util/__init__.py b/bazel_ros2_rules/ros2/tools/roslaunch_util/__init__.py deleted file mode 100644 index 12a7db48..00000000 --- a/bazel_ros2_rules/ros2/tools/roslaunch_util/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -from .roslaunch_util import ExecuteBazelTarget - -__all__ = ['ExecuteBazelTarget'] diff --git a/bazel_ros2_rules/ros2/tools/roslaunch_util/roslaunch_util.py b/bazel_ros2_rules/ros2/tools/roslaunch_util/roslaunch_util.py deleted file mode 100644 index dd02c0a2..00000000 --- a/bazel_ros2_rules/ros2/tools/roslaunch_util/roslaunch_util.py +++ /dev/null @@ -1,52 +0,0 @@ -import subprocess -import sys -import os - -from launch.actions import ExecuteProcess -from launch.frontend import expose_action - -# This util file serves as a wrapper over the cmdline -# way to run launch, using "ros2 launch launch_file.py". -# The ros_launch() bazel rule starts this script, which -# gets run as a ros_py_binary(). This way we get the ros -# environment set up for us for free. - -def find_rel_path(name, path): - for root, _, files in os.walk(path): - if name in files: - return os.path.relpath(os.path.join(root, name)) - -# Launch action to wrap over ExecuteProcess. -@expose_action('execute_bazel_target') -class ExecuteBazelTarget(ExecuteProcess): - def __init__(self,target,**kwargs): - super().__init__(cmd=[find_rel_path(target, os.getcwd())], - **kwargs) - - @classmethod - def parse(cls, entity, parser): - _, kwargs = super().parse(entity, parser, ignore=['cmd']) - kwargs['target'] = entity.get_attr('target') - return cls, kwargs - - def execute(self, context): - return super().execute(context) - -if __name__ == '__main__': - # TODO (Aditya): How do I stop installing this every time ? - # Required to get xml launch working, as we need to register an entry - # point for launch extensions. - os.system("pip install ../bazel_ros2_rules/ros2/tools/roslaunch_util >/dev/null 2>&1") - # Actual launch files should be able to import the custom action now. - env = {**os.environ, 'PYTHONPATH': os.getcwd() + '/external/bazel_ros2_rules/ros2/tools:' - + os.environ['PYTHONPATH']} - - launch_file_name = sys.argv[1] - - roslaunch_cli = "./external/ros2/ros2" - action = "launch" - # TODO : Is there a better way to locate the launch file exactly ? - launch_file = find_rel_path(launch_file_name, os.getcwd()) - - # This basically runs "ros2 launch launch_file" - subprocess.run([roslaunch_cli, action, launch_file], env=env) diff --git a/bazel_ros2_rules/ros2/tools/roslaunch_util/setup.py b/bazel_ros2_rules/ros2/tools/roslaunch_util/setup.py deleted file mode 100644 index 9c0ccfbe..00000000 --- a/bazel_ros2_rules/ros2/tools/roslaunch_util/setup.py +++ /dev/null @@ -1,10 +0,0 @@ -from setuptools import setup - -setup( - name = "roslaunch_util", - entry_points={ - 'launch.frontend.launch_extension': [ - 'roslaunch_util = roslaunch_util', - ], - } -) diff --git a/ros2_example_bazel_installed/ros2_example_apps/BUILD.bazel b/ros2_example_bazel_installed/ros2_example_apps/BUILD.bazel index ed49820d..776aa7cc 100644 --- a/ros2_example_bazel_installed/ros2_example_apps/BUILD.bazel +++ b/ros2_example_bazel_installed/ros2_example_apps/BUILD.bazel @@ -2,8 +2,7 @@ # vi: set ft=python : load("@ros2//:ros_cc.bzl", "ros_cc_binary", "ros_cc_test") -load("@ros2//:ros_py.bzl", "ros_py_binary", "ros_py_test") -load("@ros2//:ros_py.bzl", "ros_launch") +load("@ros2//:ros_py.bzl", "ros_launch", "ros_py_binary", "ros_py_test") load("@ros2//:rosidl.bzl", "rosidl_interfaces_group") load("//tools:cmd_test.bzl", "cmd_test") @@ -206,6 +205,7 @@ ros_py_binary( ros_py_binary( name = "eg_talker", srcs = ["roslaunch_eg_nodes/eg_talker.py"], + data = ["@ros2//:rmw_cyclonedds_cpp_cc"], deps = [ "@ros2//:rclpy_py", "@ros2//:std_msgs_py", @@ -215,29 +215,50 @@ ros_py_binary( ros_cc_binary( name = "eg_listener", srcs = ["roslaunch_eg_nodes/eg_listener.cpp"], + data = ["@ros2//:rmw_cyclonedds_cpp_cc"], deps = [ "@ros2//:rclcpp_cc", "@ros2//:std_msgs_cc", ], ) +# See bazel_ros2_rules/ros2/README.md, Launch Files, for notes on +# features and limitations. + # Uses a python launch file to spawn the talker and listener. +workspace_name = "ros2_example_bazel_installed" + ros_launch( name = "roslaunch_eg_py", - launch_file = "eg_launch.py", - node_targets = [ - ":eg_talker", + data = [ ":eg_listener", + ":eg_talker", ], + launch_file = "eg_launch.py", + workspace_name = workspace_name, ) # Uses an xml launch file to spawn the talker and listener. ros_launch( name = "roslaunch_eg_xml", - launch_file = "eg_launch.xml", - node_targets = [ - ":eg_talker", + data = [ ":eg_listener", + ":eg_talker", + ], + launch_file = "eg_launch.xml", + workspace_name = workspace_name, +) + +ros_py_test( + name = "roslaunch_eg_test", + srcs = ["test/roslaunch_eg_test.py"], + data = [ + ":roslaunch_eg_py", + ":roslaunch_eg_xml", + ], + main = "test/roslaunch_eg_test.py", + deps = [ + "@ros2//resources/bazel_ros_env:bazel_ros_env_py", ], ) diff --git a/ros2_example_bazel_installed/ros2_example_apps/eg_launch.py b/ros2_example_bazel_installed/ros2_example_apps/eg_launch.py index d2ed52b3..cbaff855 100644 --- a/ros2_example_bazel_installed/ros2_example_apps/eg_launch.py +++ b/ros2_example_bazel_installed/ros2_example_apps/eg_launch.py @@ -1,10 +1,18 @@ +from bazel_ros_env import Rlocation from launch import LaunchDescription -from roslaunch_util import ExecuteBazelTarget +from launch.actions import ExecuteProcess + def generate_launch_description(): + # See bazel_ros2_rules/ros2/README.md, Launch Files, for notes on + # features and limitations. + prefix = "ros2_example_bazel_installed/ros2_example_apps" + talker_bin = Rlocation(f"{prefix}/eg_talker") + listener_bin = Rlocation(f"{prefix}/eg_listener") + return LaunchDescription([ # Running a talker written in python. - ExecuteBazelTarget('eg_talker'), + ExecuteProcess(cmd=[talker_bin]), # Running a listener written in cpp. - ExecuteBazelTarget('eg_listener'), + ExecuteProcess(cmd=[listener_bin]), ]) diff --git a/ros2_example_bazel_installed/ros2_example_apps/eg_launch.xml b/ros2_example_bazel_installed/ros2_example_apps/eg_launch.xml index 07f1a926..110b1951 100644 --- a/ros2_example_bazel_installed/ros2_example_apps/eg_launch.xml +++ b/ros2_example_bazel_installed/ros2_example_apps/eg_launch.xml @@ -1,4 +1,8 @@ - - + + + diff --git a/ros2_example_bazel_installed/ros2_example_apps/roslaunch_eg_nodes/eg_listener.cpp b/ros2_example_bazel_installed/ros2_example_apps/roslaunch_eg_nodes/eg_listener.cpp index 733fcac2..be8ee77e 100644 --- a/ros2_example_bazel_installed/ros2_example_apps/roslaunch_eg_nodes/eg_listener.cpp +++ b/ros2_example_bazel_installed/ros2_example_apps/roslaunch_eg_nodes/eg_listener.cpp @@ -1,3 +1,4 @@ +#include #include #include "rclcpp/rclcpp.hpp" @@ -7,25 +8,41 @@ using std::placeholders::_1; class MinimalSubscriber : public rclcpp::Node { public: - MinimalSubscriber() - : Node("minimal_subscriber") + MinimalSubscriber(int max_count = 10) + : Node("minimal_subscriber"), max_count_(max_count) { subscription_ = this->create_subscription( "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); } + bool is_done() const { return count_ >= max_count_; } + private: - void topic_callback(const std_msgs::msg::String::SharedPtr msg) const + void topic_callback(const std_msgs::msg::String::SharedPtr msg) { RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + ++count_; } rclcpp::Subscription::SharedPtr subscription_; + + int max_count_{}; + int count_{}; }; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); + + // Use explicit `spin_some` so we can manually check for completion. + auto node = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + // Use large enough non-zero max duration to ensure we do not block. + const std::chrono::microseconds spin_max_duration{50}; + + while (rclcpp::ok() && !node->is_done()) { + executor.spin_some(spin_max_duration); + } + return 0; } diff --git a/ros2_example_bazel_installed/ros2_example_apps/roslaunch_eg_nodes/eg_talker.py b/ros2_example_bazel_installed/ros2_example_apps/roslaunch_eg_nodes/eg_talker.py index 4b3194bb..57e58e53 100644 --- a/ros2_example_bazel_installed/ros2_example_apps/roslaunch_eg_nodes/eg_talker.py +++ b/ros2_example_bazel_installed/ros2_example_apps/roslaunch_eg_nodes/eg_talker.py @@ -1,4 +1,5 @@ import rclpy +from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from std_msgs.msg import String @@ -6,34 +7,40 @@ class MinimalPublisher(Node): - def __init__(self): - super().__init__('minimal_publisher') - self.publisher_ = self.create_publisher(String, 'topic', 10) - timer_period = 0.5 # seconds + def __init__(self, *, max_count = 10): + super().__init__("node") + self.publisher = self.create_publisher(String, "topic", 10) + timer_period = 0.1 # seconds self.timer = self.create_timer(timer_period, self.timer_callback) - self.i = 0 + self.count = 0 + self.max_count = max_count def timer_callback(self): msg = String() - msg.data = 'Hello World: %d' % self.i - self.publisher_.publish(msg) - self.get_logger().info('Publishing: "%s"' % msg.data) - self.i += 1 + msg.data = f"Hello World: {self.count}" + self.publisher.publish(msg) + self.get_logger().info(f"Publishing: '{msg.data}'") + self.count += 1 + def is_done(self): + return self.count >= self.max_count -def main(args=None): - rclpy.init(args=args) - minimal_publisher = MinimalPublisher() +def main(): + rclpy.init() - rclpy.spin(minimal_publisher) + node = MinimalPublisher() - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - minimal_publisher.destroy_node() - rclpy.shutdown() + # Use explicit `spin_once` so we can manually check for completion. + executor = SingleThreadedExecutor() + executor.add_node(node) + while rclpy.ok() and not node.is_done(): + executor.spin_once(timeout_sec=1e-3) + # Avoid odd error: + # cannot use Destroyable because destruction was requested + executor._sigint_gc = None -if __name__ == '__main__': + +if __name__ == "__main__": main() diff --git a/ros2_example_bazel_installed/ros2_example_apps/test/roslaunch_eg_test.py b/ros2_example_bazel_installed/ros2_example_apps/test/roslaunch_eg_test.py new file mode 100644 index 00000000..29cd4166 --- /dev/null +++ b/ros2_example_bazel_installed/ros2_example_apps/test/roslaunch_eg_test.py @@ -0,0 +1,30 @@ +import os +import subprocess +import sys + +from bazel_ros_env import Rlocation, make_unique_ros_isolation_env + + +def test_launch_py(): + launch_bin = Rlocation( + "ros2_example_bazel_installed/ros2_example_apps/roslaunch_eg_py" + ) + subprocess.run([launch_bin], check=True) + + +def test_launch_xml(): + launch_bin = Rlocation( + "ros2_example_bazel_installed/ros2_example_apps/roslaunch_eg_xml" + ) + subprocess.run([launch_bin], check=True) + + +def main(): + os.environ.update(make_unique_ros_isolation_env()) + # For simplicity, run test points directly (rather than via pytest). + test_launch_py() + test_launch_xml() + + +if __name__ == '__main__': + main()