| @@ -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: | ||||