Not able to make dora-ros2 bridge work with example other than existing example

[bobd988]

Trying to create a dora node with python to send ROS2 node with new message types but failed.

[phil-opp]

Could you clarify what you mean with "new message types"? Also, what kind of error occurs?

[bobd988]

Here is an existing ROS C++ example that can send rviz related message to rviz. We are trying to do same thing from dora to rviz. Instead of trial-and-error learning, Could you show how to convert this to Dora and send to Rviz?

https://wiki.ros.org/rviz/Tutorials/Markers%3A%20Basic%20Shapes

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

int main( int argc, char** argv )
{
  ros::init(argc, argv, "basic_shapes");
  ros::NodeHandle n;
  ros::Rate r(1);
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);

  // Set our initial shape type to be a cube
  uint32_t shape = visualization_msgs::Marker::CUBE;

  while (ros::ok())
  {
    visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "/my_frame";
    marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    marker.ns = "basic_shapes";
    marker.id = 0;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = shape;

    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    marker.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
    marker.pose.position.x = 0;
    marker.pose.position.y = 0;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

    marker.lifetime = ros::Duration();

    // Publish the marker
    while (marker_pub.getNumSubscribers() < 1)
    {
      if (!ros::ok())
      {
        return 0;
      }
      ROS_WARN_ONCE("Please create a subscriber to the marker");
      sleep(1);
    }
    marker_pub.publish(marker);

    // Cycle between different shapes
    switch (shape)
    {
    case visualization_msgs::Marker::CUBE:
      shape = visualization_msgs::Marker::SPHERE;
      break;
    case visualization_msgs::Marker::SPHERE:
      shape = visualization_msgs::Marker::ARROW;
      break;
    case visualization_msgs::Marker::ARROW:
      shape = visualization_msgs::Marker::CYLINDER;
      break;
    case visualization_msgs::Marker::CYLINDER:
      shape = visualization_msgs::Marker::CUBE;
      break;
    }

    r.sleep();
  }
}

[phil-opp]

I'm still working on some remaining functionality, e.g. exposing specified constants in the API. In general, the API will look something like this:

auto ros2_context = init_ros2_context();
auto node = ros2_context->new_node(namespace, name);
auto topic = node->create_topic_visualization_msgs_Marker(namespace, "visualization_marker", qos);
auto publisher = node->create_publisher(topic, qos);

visualization_msgs::Marker marker;
marker.header.frame_id = "/my_frame";
marker.id = 0;
// etc

publisher->publish(marker);

[haixuanTao]

In python, the following example should be able to create a marker:

# demo_pub_marker_data.py

from typing import Callable, Optional
import pyarrow as pa
from dora import DoraStatus
import dora
import numpy as np

marker = pa.array(
    [
        {
            "header": {
                "frame_id": "world",  # Placeholder value (String type, no numpy equivalent)
            },
            "ns": "my_namespace",  # Placeholder value (String type, no numpy equivalent)
            "id": np.int32(1),  # Numpy type
            "type": np.int32(0),  # Numpy type (ARROW)
            "action": np.int32(0),  # Numpy type (ADD)
            "lifetime": {
                "sec": np.int32(1),
                "nanosec": np.uint32(2),
            },  # Numpy type
            "pose": {
                "position": {
                    "x": np.float64(1.0),  # Numpy type
                    "y": np.float64(2.0),  # Numpy type
                    "z": np.float64(3.0),  # Numpy type
                },
                "orientation": {
                    "x": np.float64(0.0),  # Numpy type
                    "y": np.float64(0.0),  # Numpy type
                    "z": np.float64(0.0),  # Numpy type
                    "w": np.float64(1.0),  # Numpy type
                },
            },
            "scale": {
                "x": np.float64(1.0),  # Numpy type
                "y": np.float64(1.0),  # Numpy type
                "z": np.float64(1.0),  # Numpy type
            },
            "color": {
                "r": np.float32(1.0),  # Numpy type
                "g": np.float32(0.0),  # Numpy type
                "b": np.float32(0.0),  # Numpy type
                "a": np.float32(1.0),  # Numpy type (alpha)
            },
            "frame_locked": False,  # Boolean type, no numpy equivalent
            "points": [  # Numpy array for points
                {
                    "x": np.float64(1.0),  # Numpy type
                    "y": np.float64(1.0),  # Numpy type
                    "z": np.float64(1.0),  # Numpy type
                }
            ],
            "colors": [
                {
                    "r": np.float32(1.0),  # Numpy type
                    "g": np.float32(1.0),  # Numpy type
                    "b": np.float32(1.0),  # Numpy type
                    "a": np.float32(1.0),  # Numpy type (alpha)
                }  # Numpy array for colors
            ],
            "texture_resource": "",
            "uv_coordinates": [{}],
            "text": "",
            "mesh_resource": "",
            "mesh_use_embedded_materials": False,  # Boolean type, no numpy equivalent
        }
    ]
)


class Operator:
    def __init__(self) -> None:
        self.ros2_context = dora.experimental.ros2_bridge.Ros2Context()
        # create ros2 node
        self.ros2_node = self.ros2_context.new_node(
            "marker2ros",
            "/ros2_bridge",
            dora.experimental.ros2_bridge.Ros2NodeOptions(rosout=True),
        )
        # create ros2 qos
        self.topic_qos = dora.experimental.ros2_bridge.Ros2QosPolicies(
            reliable=True, max_blocking_time=0.1
        )
        # create ros2 topic
        self.marker_data_topic = self.ros2_node.create_topic(
            "/ros2_bridge/visualization_marker",
            "visualization_msgs/Marker",
            self.topic_qos,
        )
        # create ros2 publisher
        self.marker_data_publisher = self.ros2_node.create_publisher(
            self.marker_data_topic
        )

    def on_event(
        self,
        dora_event,
        send_output,
    ) -> DoraStatus:
        if dora_event["type"] == "INPUT":
            # print(pa.array([marker_data_dict]))
            self.marker_data_publisher.publish(marker)
        return DoraStatus.CONTINUE

Could you try it out as I don't have a ubuntu version that works with ros2?