Status Update: 2023-08-30 #344
haixuanTao
started this conversation in
Status Update
Replies: 5 comments 5 replies
-
dora-ros2-bridge
import dora
from dora import Node
import pyarrow as pa
ros2_context = dora.experimental.ros2_bridge.Ros2Context()
ros2_node = ros2_context.new_node(
"turtle_teleop",
"/ros2_demo",
dora.experimental.ros2_bridge.Ros2NodeOptions(rosout=True),
)
topic_qos = dora.experimental.ros2_bridge.Ros2QosPolicies(
reliable=True, max_blocking_time=0.1
)
turtle_twist_topic = ros2_node.create_topic(
"/turtle1/cmd_vel", "geometry_msgs::Twist", topic_qos
)
twist_writer = ros2_node.create_publisher(turtle_twist_topic)
turtle_pose_topic = ros2_node.create_topic(
"/turtle1/pose", "turtlesim::Pose", topic_qos
)
pose_reader = ros2_node.create_subscription(turtle_pose_topic)
dora_node = Node()
dora_node.merge_external_events(pose_reader)
for event in dora_node.next():
match event["kind"]:
case "dora":
match event["type"]:
case "INPUT":
match event["id"]:
case "direction":
direction = {
"linear": {
"x": event["data"][0],
},
"angular": {
"z": event["data"][5],
},
}
twist_writer.publish(pa.array(direction))
case "external":
pose = event.inner()[0].as_py()
dora_node.send_output(
"turtle_pose",
pa.array(
[
pose["x"],
pose["y"],
pose["theta"],
pose["linear_velocity"],
pose["angular_velocity"],
],
type=pa.float64(),
),
)
BasicType::I8 => Int8Array
BasicType::I16 => Int16Array
BasicType::I32 => Int32Array
BasicType::I64 => Int64Array
BasicType::U8 => UInt8Array
BasicType::U16 => UInt16Array
BasicType::U32 => UInt32Array
BasicType::U64 => UInt64Array
BasicType::F32 => Float32Array
BasicType::F64 => Float64Array
BasicType::Char => StringArray
BasicType::Byte => UInt8Array
BasicType::Bool => BooleanArray
NestableType::GenericString(_) => StringArray
NestableType::NamedType(name) => StructArray or fn main() -> eyre::Result<()> {
let mut ros_node = init_ros_node()?;
let turtle_vel_publisher = create_vel_publisher(&mut ros_node)?;
let turtle_pose_reader = create_pose_reader(&mut ros_node)?;
let output = DataId::from("pose".to_owned());
let (mut node, dora_events) = DoraNode::init_from_env()?;
let merged = dora_events.merge_external(Box::pin(turtle_pose_reader.async_stream()));
let mut events = futures::executor::block_on_stream(merged);
for let Some(event) = match events.next() {
MergedEvent::Dora(event) => match event {
Event::Input {
id,
metadata: _,
data: _,
} => match id.as_str() {
"tick" => {
let direction = Twist {
linear: Vector3 {
x: rand::random::<f64>() + 1.0,
..Default::default()
},
angular: Vector3 {
z: (rand::random::<f64>() - 0.5) * 5.0,
..Default::default()
},
};
turtle_vel_publisher.publish(direction).unwrap();
}
other => eprintln!("Ignoring unexpected input `{other}`"),
},
other => eprintln!("Received unexpected input: {other:?}"),
},
MergedEvent::External(pose) => {
if let Ok((pose, _)) = pose {
let serialized = serde_json::to_string(&pose)?;
node.send_output_bytes(
output.clone(),
Default::default(),
serialized.len(),
serialized.as_bytes(),
)?;
}
}
}
}
Ok(())
} |
Beta Was this translation helpful? Give feedback.
1 reply
-
|
Beta Was this translation helpful? Give feedback.
0 replies
-
|
Beta Was this translation helpful? Give feedback.
0 replies
-
Future Action:
|
Beta Was this translation helpful? Give feedback.
0 replies
-
Working on a discord server
|
Beta Was this translation helpful? Give feedback.
4 replies
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
-
👍
Beta Was this translation helpful? Give feedback.
All reactions