dora-rs / dora

DORA (Dataflow-Oriented Robotic Application) is middleware designed to streamline and simplify the creation of AI-based robotic applications. It offers low latency, composable, and distributed dataflow capabilities. Applications are modeled as directed graphs, also referred to as pipelines.
https://dora-rs.ai
Apache License 2.0
1.36k stars 69 forks source link

Add ROS2 bridge support for C++ nodes #425

Closed phil-opp closed 4 months ago

phil-opp commented 4 months ago

Build and Usage Instructions

Usage Example:

    auto dora_node = init_dora_node();
    auto merged_events = dora_events_into_combined(std::move(dora_node.events));

    auto qos = qos_default();
    qos.durability = Ros2Durability::Volatile;
    qos.liveliness = Ros2Liveliness::Automatic;
    qos.reliable = true;
    qos.max_blocking_time = 0.1;

    auto ros2_context = init_ros2_context();
    auto node = ros2_context->new_node("/ros2_demo", "turtle_teleop");
    auto vel_topic = node->create_topic_geometry_msgs_Twist("/turtle1", "cmd_vel", qos);
    auto vel_publisher = node->create_publisher(vel_topic, qos);
    auto pose_topic = node->create_topic_turtlesim_Pose("/turtle1", "pose", qos);
    auto pose_subscription = node->create_subscription(pose_topic, qos, merged_events);

    auto event = merged_events.next();

    if (event.is_dora())
    {
        auto dora_event = downcast_dora(std::move(event)); 
        ...
        geometry_msgs::Twist twist = {
                    .linear = {.x = 1, .y = 0, .z = 0},
                    .angular = {.x = 0, .y = 0, .z = 1}
        };
        vel_publisher->publish(twist);
    }
    else if (pose_subscription->matches(event))
    {
        auto pose = pose_subscription->downcast(std::move(event));
        std::cout << "Received pose x:" << pose.x << ", y:" << pose.y << std::endl;
    }

Also part of this PR:

phil-opp commented 4 months ago

I'm still working on fixing the build on the Windows CI runner, which uses a non-standard target dir. Otherwise, this should be ready for an initial round of testing. I tried a simple example with the turtlesim and both publish and subscribe seem to work.

phil-opp commented 4 months ago

I'm still working on fixing the build on the Windows CI runner, which uses a non-standard target dir.

This should be fixed with https://github.com/dora-rs/dora/pull/425/commits/3b1777c2e240c72101900ebb922b4a990d38a999.

phil-opp commented 4 months ago

For example, the generated struct for the Twist message looks like this:

struct Twist final {
  ::geometry_msgs::Vector3 linear;
  ::geometry_msgs::Vector3 angular;

  bool operator==(Twist const &) const noexcept;
  bool operator!=(Twist const &) const noexcept;
  using IsRelocatable = ::std::true_type;
};
phil-opp commented 4 months ago

I added some usage instructions in 70fb49b. I think this PR is ready for another round of review.

There are still multiple things that we want to improve, but I think it's better to do this in follow-up PRs.