feat: shutdown gracefully (#13)

* feat: shutdown gracefully

* ci: fix the test - full round trip - no hanging

ci_test.sh

@@ -1,8 +1,13 @@
 #!/bin/bash
+set -e
+set -x
+
 cargo build
 roslaunch launch/test_launch.launch &
 sleep 2
 cargo run --example amiga-mock-server -- --port 50050 &
-cargo run -- --test-mode --port 50050 &
-cargo run --example test_amiga_vel_publisher_subscriber
+sleep 2
+cargo run -- --port 50050 &
+sleep 2
+cargo run --example test_amiga_vel_pubsub
 echo DONE

examples/amiga-mock-server.rs

@@ -81,7 +81,7 @@ impl CanbusService for AmigaMockService {
                 match item {
                     Ok(v) => {
                         let state = v.command.clone().unwrap();
-                        debug!("state received: {:?}", state);
+                        info!("cmd received: {:?}", state);
 
                         let twist = Twist2d {
                             linear_velocity_x: state.linear_velocity_x,
@@ -139,6 +139,8 @@ impl CanbusService for AmigaMockService {
                 }))
                 .await
                 .unwrap();
+                // throttle to 20Hz
+                tokio::time::sleep(std::time::Duration::from_millis(50)).await;
             }
         });
 
@@ -160,7 +162,7 @@ struct Args {
 #[tokio::main]
 async fn main() -> Result<(), Box<dyn std::error::Error>> {
     let subscriber = FmtSubscriber::builder()
-        .with_max_level(Level::DEBUG)
+        .with_max_level(Level::INFO)
         .finish();
     tracing::subscriber::set_global_default(subscriber).expect("setting default subscriber failed");
 
@@ -171,7 +173,7 @@ async fn main() -> Result<(), Box<dyn std::error::Error>> {
     let server = AmigaMockService {
         twist_state: Arc::new(Mutex::new(Twist2d::default())),
     };
-    info!("Trying to start server.");
+    debug!("Trying to start server.");
 
     Server::builder()
         .add_service(CanbusServiceServer::new(server))

examples/amiga_vel_subscriber.rs

@@ -4,7 +4,7 @@ use tracing_subscriber::FmtSubscriber;
 fn main() {
     tracing::subscriber::set_global_default(
         FmtSubscriber::builder()
-            .with_max_level(Level::DEBUG)
+            .with_max_level(Level::INFO)
             .finish(),
     )
     .expect("setting default subscriber failed");
@@ -30,9 +30,6 @@ fn main() {
     .unwrap();
     info!("amiga_vel_subscriber launched");
 
-    // Block the thread until a count=10
-
-    //loop {
     while rosrust::is_ok() {
         // Sleep to maintain 20Hz rate
         rate.sleep();

examples/test_amiga_vel_pubsub.rs

@@ -0,0 +1,72 @@
+use tracing::{info, Level};
+use tracing_subscriber::FmtSubscriber;
+
+fn main() {
+    tracing::subscriber::set_global_default(
+        FmtSubscriber::builder()
+            .with_max_level(Level::DEBUG)
+            .finish(),
+    )
+    .expect("setting default subscriber failed");
+
+    // Initialize node
+    rosrust::init("amiga_vel_subscriber");
+
+    // Create object that maintains 20Hz between sleep requests
+    let rate = rosrust::rate(20.0);
+
+    let shared_state = std::sync::Arc::new(std::sync::Mutex::<
+        rosrust_msg::geometry_msgs::TwistStamped,
+    >::default());
+    let shared_state_clone = shared_state.clone();
+
+    let _subscriber_raii = rosrust::subscribe(
+        "/amiga/vel",
+        100,
+        move |v: rosrust_msg::geometry_msgs::TwistStamped| {
+            info!("Received: {:?}", v);
+            let mut data = shared_state_clone.lock().unwrap();
+            *data = v;
+        },
+    )
+    .unwrap();
+    info!("amiga_vel_subscriber launched");
+    let cmd_vel_pub = rosrust::publish("/amiga/cmd_vel", 2).unwrap();
+
+    let mut pub_num = 0;
+    while rosrust::is_ok() {
+        info!("HERE!!!");
+
+        // Create string message
+        let msg = rosrust_msg::geometry_msgs::Twist {
+            linear: rosrust_msg::geometry_msgs::Vector3 {
+                x: pub_num as f64 * 0.1,
+                y: 0.0,
+                z: 0.0,
+            },
+            angular: rosrust_msg::geometry_msgs::Vector3 {
+                x: 0.0,
+                y: 0.0,
+                z: 0.5,
+            },
+        };
+
+        // Send string message to topic via publisher
+        cmd_vel_pub.send(msg).unwrap();
+        pub_num += 1;
+
+        // Sleep to maintain 20Hz rate
+        rate.sleep();
+        let state = shared_state.lock().unwrap().clone();
+        info!("amiga_vel_subscriber got: {:?}", state);
+
+        let x = state.twist.linear.x;
+        let theta = state.twist.angular.z;
+
+        if x >= 1.0 && (theta - 0.5).abs() < 0.001 {
+            // We received twist state which achieved linear and angular vel as commanded.
+            break;
+        }
+    }
+    info!("amiga_vel_subscriber done");
+}