diff --git a/Cargo.toml b/Cargo.toml index 4cb4fe72..73df58d3 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -8,6 +8,7 @@ edition = "2021" [[example]] name = "random_turtle" +path = "examples/rust-standalone-bridge/random_turtle.rs" required-features = ["ros2-examples"] # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html diff --git a/examples/python-dataflow/random_turtle.py b/examples/python-dataflow/random_turtle.py new file mode 100644 index 00000000..75d909e8 --- /dev/null +++ b/examples/python-dataflow/random_turtle.py @@ -0,0 +1,37 @@ +import time, random +from dora_ros2_bridge import * +from dora import Node + +node = Node() +context = Ros2Context() +node = context.new_node("turtle_teleop", "/ros2_demo", Ros2NodeOptions(rosout=True)) + +topic_qos = Ros2QosPolicies(reliable=True, max_blocking_time=0.1) + +turtle_twist_topic = node.create_topic( + "/turtle1/cmd_vel", "geometry_msgs::Twist", topic_qos +) +twist_writer = node.create_publisher(turtle_twist_topic) + +turtle_pose_topic = node.create_topic("/turtle1/pose", "turtlesim::Pose", topic_qos) +pose_reader = node.create_subscription(turtle_pose_topic) + +for event in node: + match event["type"]: + case "INPUT": + match event["id"]: + case "direction": + direction = { + "linear": { + "x": event["data"][0], + }, + "angular": { + "z": event["data"][5], + }, + } + twist_writer.publish(direction) + case "tick": + pose = pose_reader.next() + if pose == None: + break + node.send_output(pose) diff --git a/examples/random_turtle.rs b/examples/rust-standalone-bridge/random_turtle.rs similarity index 100% rename from examples/random_turtle.rs rename to examples/rust-standalone-bridge/random_turtle.rs diff --git a/python/random_turtle.py b/python/random_turtle.py deleted file mode 100644 index 2b68dd2d..00000000 --- a/python/random_turtle.py +++ /dev/null @@ -1,30 +0,0 @@ -from dora_ros2_bridge import * -import time, random - -context = Ros2Context() -node = context.new_node("turtle_teleop", "/ros2_demo", Ros2NodeOptions(rosout = True)) - -topic_qos = Ros2QosPolicies(reliable = True, max_blocking_time = 0.1) - -turtle_twist_topic = node.create_topic("/turtle1/cmd_vel", "geometry_msgs::Twist", topic_qos) -twist_writer = node.create_publisher(turtle_twist_topic) - -turtle_pose_topic = node.create_topic("/turtle1/pose", "turtlesim::Pose", topic_qos) -pose_reader = node.create_subscription(turtle_pose_topic) - -for i in range(500): - direction = { - 'linear': { - 'x': random.random() + 1, - }, - 'angular': { - 'z': (random.random() - 0.5) * 5, - } - } - twist_writer.publish(direction); - while True: - pose = pose_reader.next() - if pose == None: - break - print(pose) - time.sleep(0.5)