Browse Source

Add documentation to the ROS2 bridge

tags/v0.3.0-rc
haixuanTao 2 years ago
parent
commit
1a2eabe080
1 changed files with 9 additions and 3 deletions
  1. +9
    -3
      examples/python-ros2-dataflow/random_turtle.py

+ 9
- 3
examples/python-ros2-dataflow/random_turtle.py View File

@@ -1,14 +1,12 @@
#!/usr/bin/env python #!/usr/bin/env python
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-


import time
import random
import dora import dora
from dora import Node from dora import Node
import pyarrow as pa


CHECK_TICK = 50 CHECK_TICK = 50


# Create a ROS2 Context
ros2_context = dora.experimental.ros2_bridge.Ros2Context() ros2_context = dora.experimental.ros2_bridge.Ros2Context()
ros2_node = ros2_context.new_node( ros2_node = ros2_context.new_node(
"turtle_teleop", "turtle_teleop",
@@ -16,21 +14,27 @@ ros2_node = ros2_context.new_node(
dora.experimental.ros2_bridge.Ros2NodeOptions(rosout=True), dora.experimental.ros2_bridge.Ros2NodeOptions(rosout=True),
) )


# Define a ROS2 QOS
topic_qos = dora.experimental.ros2_bridge.Ros2QosPolicies( topic_qos = dora.experimental.ros2_bridge.Ros2QosPolicies(
reliable=True, max_blocking_time=0.1 reliable=True, max_blocking_time=0.1
) )


# Create a publisher to cmd_vel topic
turtle_twist_topic = ros2_node.create_topic( turtle_twist_topic = ros2_node.create_topic(
"/turtle1/cmd_vel", "geometry_msgs::Twist", topic_qos "/turtle1/cmd_vel", "geometry_msgs::Twist", topic_qos
) )
twist_writer = ros2_node.create_publisher(turtle_twist_topic) twist_writer = ros2_node.create_publisher(turtle_twist_topic)


# Create a listener to pose topic
turtle_pose_topic = ros2_node.create_topic( turtle_pose_topic = ros2_node.create_topic(
"/turtle1/pose", "turtlesim::Pose", topic_qos "/turtle1/pose", "turtlesim::Pose", topic_qos
) )
pose_reader = ros2_node.create_subscription(turtle_pose_topic) pose_reader = ros2_node.create_subscription(turtle_pose_topic)


# Create a dora node
dora_node = Node() dora_node = Node()

# Listen for both stream on the same loop as Python does not handle well multiprocessing
dora_node.merge_external_events(pose_reader) dora_node.merge_external_events(pose_reader)


print("looping", flush=True) print("looping", flush=True)
@@ -40,6 +44,7 @@ for i in range(500):
if event is None: if event is None:
break break
match event["kind"]: match event["kind"]:
# Dora event
case "dora": case "dora":
match event["type"]: match event["type"]:
case "INPUT": case "INPUT":
@@ -47,6 +52,7 @@ for i in range(500):
case "direction": case "direction":
twist_writer.publish(event["value"]) twist_writer.publish(event["value"])


# ROS2 Event
case "external": case "external":
pose = event.inner()[0].as_py() pose = event.inner()[0].as_py()
if i == CHECK_TICK: if i == CHECK_TICK:


Loading…
Cancel
Save