diff --git a/bazel_ros2_rules/ros2/README.md b/bazel_ros2_rules/ros2/README.md index bf71c1394..556b3dfd0 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` @@ -192,5 +211,8 @@ From the above two examples (at present), the following features are in The other repos, however, have the following that `bazel_ros2_rules` is missing: -- Affordances for `ros2 launch` - Launching from containers (Docker, Apptainer) + +Some common features might be: + +- Bazel affordances for `ros2 launch` 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 d5c73935c..41d1f6ca5 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 619d7ae08..c20c18025 100644 --- a/bazel_ros2_rules/ros2/ros_py.bzl +++ b/bazel_ros2_rules/ros2/ros_py.bzl @@ -130,6 +130,102 @@ 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, + 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) + + 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 = main, + deps = deps, + srcs = [main], + data = data, + visibility = visibility, + **kwargs + ) + def ros_py_test( name, rmw_implementation = None, diff --git a/ros2_example_bazel_installed/ros2_example_apps/BUILD.bazel b/ros2_example_bazel_installed/ros2_example_apps/BUILD.bazel index 0c7d534b4..776aa7cc3 100644 --- a/ros2_example_bazel_installed/ros2_example_apps/BUILD.bazel +++ b/ros2_example_bazel_installed/ros2_example_apps/BUILD.bazel @@ -2,7 +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", "ros_py_binary", "ros_py_test") load("@ros2//:rosidl.bzl", "rosidl_interfaces_group") load("//tools:cmd_test.bzl", "cmd_test") @@ -201,6 +201,67 @@ ros_py_binary( ], ) +# ROS launch example. +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", + ], +) + +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", + 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", + 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", + ], +) + ros_py_test( name = "custom_message_rosbag_test", srcs = ["test/custom_message_rosbag_test.py"], diff --git a/ros2_example_bazel_installed/ros2_example_apps/README.md b/ros2_example_bazel_installed/ros2_example_apps/README.md index 23b3e912c..39e5d6700 100644 --- a/ros2_example_bazel_installed/ros2_example_apps/README.md +++ b/ros2_example_bazel_installed/ros2_example_apps/README.md @@ -16,3 +16,27 @@ bazel run //ros2_example_apps:oracle_py ```sh bazel run //ros2_example_apps:inquirer_cc ``` + +### Example Launch Files + +See `bazel_ros2_rules/ros2/README.md`, Launch Files, for notes on +features and limitations. + +Example Python launch file: + +```sh +bazel run //ros2_example_apps:roslaunch_eg_py + +# Or +bazel build //ros2_example_apps:roslaunch_eg_py +./bazel-bin/ros2_example_apps/roslaunch_eg_py +``` + +Example XML launch file: + +```sh +bazel run //ros2_example_apps:roslaunch_eg_xml +``` + +WARNING: Per notes in `bazel_ros2_rules/ros2/README.md`, the XML launch file +will not resolve paths correctly. diff --git a/ros2_example_bazel_installed/ros2_example_apps/eg_launch.py b/ros2_example_bazel_installed/ros2_example_apps/eg_launch.py new file mode 100644 index 000000000..cbaff855f --- /dev/null +++ b/ros2_example_bazel_installed/ros2_example_apps/eg_launch.py @@ -0,0 +1,18 @@ +from bazel_ros_env import Rlocation +from launch import LaunchDescription +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. + ExecuteProcess(cmd=[talker_bin]), + # Running a listener written in cpp. + 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 new file mode 100644 index 000000000..110b1951c --- /dev/null +++ b/ros2_example_bazel_installed/ros2_example_apps/eg_launch.xml @@ -0,0 +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 new file mode 100644 index 000000000..be8ee77e8 --- /dev/null +++ b/ros2_example_bazel_installed/ros2_example_apps/roslaunch_eg_nodes/eg_listener.cpp @@ -0,0 +1,48 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +using std::placeholders::_1; + +class MinimalSubscriber : public rclcpp::Node +{ + public: + 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) + { + 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); + + // 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 new file mode 100644 index 000000000..0e9bfaeee --- /dev/null +++ b/ros2_example_bazel_installed/ros2_example_apps/roslaunch_eg_nodes/eg_talker.py @@ -0,0 +1,42 @@ +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node + +from std_msgs.msg import String + + +class MinimalPublisher(Node): + + 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.count = 0 + self.max_count = max_count + + def timer_callback(self): + msg = String() + 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(): + rclpy.init() + + node = MinimalPublisher() + + # 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) + + +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 000000000..29cd41664 --- /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()