From 3f156e05d87f81cfde4f8d04efce0aed9aa0afca Mon Sep 17 00:00:00 2001
From: Philipp Oppermann <dev@phil-opp.com>
Date: Fri, 23 Jun 2023 21:00:20 +0200
Subject: [PATCH] Add example to move turtle in turtlesim in random directions

---
 Cargo.lock                |   2 +
 Cargo.toml                |   4 ++
 examples/random_turtle.rs | 104 ++++++++++++++++++++++++++++++++++++++
 3 files changed, 110 insertions(+)
 create mode 100644 examples/random_turtle.rs

diff --git a/Cargo.lock b/Cargo.lock
index 1fc97853..c69c877f 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -143,6 +143,8 @@ version = "0.1.0"
 dependencies = [
  "array-init",
  "dora-ros2-bridge-msg-gen",
+ "futures",
+ "rand",
  "ros2-client",
  "rustdds",
  "serde",
diff --git a/Cargo.toml b/Cargo.toml
index b4f99435..04522866 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -16,3 +16,7 @@ serde-big-array = "0.5.1"
 widestring = "1.0.2"
 ros2-client = "0.5.2"
 rustdds = "0.8.3"
+
+[dev-dependencies]
+rand = "0.8.5"
+futures = { version = "0.3.28", default-features = false }
diff --git a/examples/random_turtle.rs b/examples/random_turtle.rs
new file mode 100644
index 00000000..b24ebe97
--- /dev/null
+++ b/examples/random_turtle.rs
@@ -0,0 +1,104 @@
+//! Instructions:
+//!
+//! - Source the ROS2 setup files (e.g. `source /opt/ros/iron/setup.bash`)
+//! - Run `ros2 run turtlesim turtlesim_node`
+//! - Open a second terminal and source the ROS2 setup files again.
+//! - Run this example to move the turtle in random directions: `cargo run --example random_turtle`
+
+use std::time::Duration;
+
+use dora_ros2_bridge::{
+    self,
+    geometry_msgs::msg::{Twist, Vector3},
+    ros2_client::{self, ros2, NodeOptions},
+    rustdds::{self, policy},
+    turtlesim::srv::SetPen_Request,
+};
+use futures::FutureExt;
+
+fn main() {
+    let ros_context = ros2_client::Context::new().unwrap();
+
+    let mut ros_node = ros_context
+        .new_node(
+            "turtle_teleop", // name
+            "/ros2_demo",    // namespace
+            NodeOptions::new().enable_rosout(true),
+        )
+        .unwrap();
+
+    let topic_qos: rustdds::QosPolicies = {
+        rustdds::QosPolicyBuilder::new()
+            .durability(policy::Durability::Volatile)
+            .liveliness(policy::Liveliness::Automatic {
+                lease_duration: ros2::Duration::DURATION_INFINITE,
+            })
+            .reliability(policy::Reliability::Reliable {
+                max_blocking_time: ros2::Duration::from_millis(100),
+            })
+            .history(policy::History::KeepLast { depth: 1 })
+            .build()
+    };
+
+    let turtle_cmd_vel_topic = ros_node
+        .create_topic(
+            "/turtle1/cmd_vel",
+            String::from("geometry_msgs::msg::dds_::Twist_"),
+            &topic_qos,
+        )
+        .unwrap();
+
+    // The point here is to publish Twist for the turtle
+    let turtle_cmd_vel_writer = ros_node
+        .create_publisher::<Twist>(&turtle_cmd_vel_topic, None)
+        .unwrap();
+
+    let turtle_pose_topic = ros_node
+        .create_topic(
+            "/turtle1/pose",
+            String::from("turtlesim::msg::dds_::Pose_"),
+            &Default::default(),
+        )
+        .unwrap();
+    let turtle_pose_reader = ros_node
+        .create_subscription::<dora_ros2_bridge::turtlesim::msg::Pose>(&turtle_pose_topic, None)
+        .unwrap();
+
+    for _ in 0..100 {
+        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()
+            },
+        };
+        println!("sending {direction:?}");
+        turtle_cmd_vel_writer.publish(direction).unwrap();
+        std::thread::sleep(Duration::from_millis(500));
+
+        while let Some(Ok((pose, _info))) = turtle_pose_reader.async_take().now_or_never() {
+            println!("{pose:?}");
+        }
+    }
+}
+
+#[derive(Debug)]
+pub enum RosCommand {
+    StopEventLoop,
+    TurtleCmdVel {
+        turtle_id: i32,
+        twist: Twist,
+    },
+    Reset,
+    SetPen(SetPen_Request),
+    Spawn(String),
+    Kill(String),
+    RotateAbsolute {
+        heading: f32,
+    },
+    #[allow(non_camel_case_types)]
+    RotateAbsolute_Cancel,
+}