action servers

actions/build.rs

@@ -62,16 +62,14 @@ fn main() {
         .whitelist_recursively(false)
         .whitelist_type("rcl_action_client_t").opaque_type("rcl_action_client_t")
         .whitelist_type("rcl_action_server_t").opaque_type("rcl_action_server_t")
-        .whitelist_type("rcl_action_goal_info_t").opaque_type("rcl_action_goal_info_t")
+        .whitelist_type("rcl_action_goal_info_t")
         .whitelist_type("rcl_action_goal_handle_t").opaque_type("rcl_action_goal_handle_t")
-        .whitelist_type("rcl_action_cancel_request_t").opaque_type("rcl_action_cancel_request_t")
-        .whitelist_type("rcl_action_cancel_response_t").opaque_type("rcl_action_cancel_response_t")
-        .whitelist_type("rcl_action_goal_info_t").opaque_type("rcl_action_goal_info_t")
-        .whitelist_type("rcl_action_goal_event_t").opaque_type("rcl_action_goal_event_t")
+        .whitelist_type("rcl_action_cancel_request_t")
+        .whitelist_type("rcl_action_cancel_response_t")
+        .whitelist_type("rcl_action_goal_event_t")
         .whitelist_type("rcl_action_goal_state_t").opaque_type("rcl_action_goal_state_t")
         .whitelist_type("rcl_action_goal_status_array_t").opaque_type("rcl_action_goal_status_array_t")
         .whitelist_function("rcl_action_.*")
-        // .whitelist_type("rosidl_action_type_support_t").opaque_type("rosidl_action_type_support_t")
         .whitelist_type("rcl_action_client_options_t")
         .whitelist_type("rcl_action_server_options_t")
         .generate()

examples/action_client.rs

@@ -30,31 +30,32 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
     let task_spawner = spawner.clone();
     let task_done = done.clone();
     spawner.spawn_local(async move {
-        let goal = goal_fut.await.unwrap(); // assume success
+        let mut goal = goal_fut.await.unwrap(); // assume success
 
+        println!("goal accepted: {}", goal.uuid);
         // process feedback stream in its own task
-        let goal_id = goal.uuid.clone();
+        let nested_goal = goal.clone();
         let nested_task_done = task_done.clone();
         task_spawner
-            .spawn_local(goal.feedback.for_each(move |msg| {
-                let task_client = client.clone();
-                let task_done = nested_task_done.clone();
+            .spawn_local(goal.get_feedback().unwrap().for_each(move |msg| {
+                let nested_task_done = nested_task_done.clone();
+                let nested_goal = nested_goal.clone();
                 async move {
-                    println!("new feedback msg {:?}", msg);
+                    println!("new feedback msg {:?} -- {:?}", msg, nested_goal.get_status());
 
                     // cancel the goal before it finishes. (comment out to complete the goal)
                     if msg.sequence.len() == 8 {
-                        task_client.lock().unwrap().send_cancel_request(&goal_id).unwrap().
+                        nested_goal.cancel().unwrap().
                             map(|r| {
                                 println!("goal cancelled: {:?}", r);
                                 // we are done.
-                                *task_done.lock().unwrap() = true;
+                                *nested_task_done.lock().unwrap() = true;
                             }).await;
                     }
                 }})).unwrap();
 
         // await result in this task
-        let result = goal.result.await;
+        let result = goal.get_result().unwrap().await;
         match result {
             Ok(msg) => {
                 println!("got result {:?}", msg);

examples/action_server.rs

@@ -0,0 +1,90 @@
+use futures::executor::LocalPool;
+use futures::task::LocalSpawnExt;
+use std::sync::{Arc,Mutex};
+use r2r;
+use r2r::ServerGoal;
+use r2r::example_interfaces::action::Fibonacci;
+
+// note: cannot be blocking.
+fn accept_goal_cb(uuid: &uuid::Uuid, goal: &Fibonacci::Goal) -> bool {
+    println!("Got goal request with order {}, goal id: {}", goal.order, uuid);
+    // reject high orders
+    goal.order < 100
+}
+
+// note: cannot be blocking.
+fn accept_cancel_cb(goal: &ServerGoal<Fibonacci::Action>) -> bool {
+    println!("Got request to cancel {}", goal.uuid);
+    // always accept cancel requests
+    true
+}
+
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    let mut pool = LocalPool::new();
+    let spawner = pool.spawner();
+
+    let task_spawner = spawner.clone();
+
+    let ctx = r2r::Context::create()?;
+    let node = Arc::new(Mutex::new(r2r::Node::create(ctx, "testnode", "")?));
+
+    // signal that we are done
+    let done = Arc::new(Mutex::new(false));
+
+    let node_cb = node.clone();
+    let done_cb = done.clone();
+    let handle_goal_cb = move |mut g: ServerGoal<Fibonacci::Action>| {
+        // note that we cannot create the timer here, since we are
+        // called during spin_once which menas whoever is spinning holds the mutex.
+        // instead we just set up and immediately start a task.
+        // also we cannot block which is why we spawn the task
+        let node_cb = node_cb.clone();
+        let done_cb = done_cb.clone();
+        task_spawner.spawn_local(async move {
+            let mut timer = node_cb.lock().unwrap().create_wall_timer(std::time::Duration::from_millis(1000))
+                .expect("could not create timer");
+            let mut feedback_msg = Fibonacci::Feedback {
+                sequence: vec![0,1],
+            };
+            g.publish_feedback(feedback_msg.clone()).expect("fail");
+            let order = g.goal.order as usize;
+            for i in 1..order {
+                if g.is_cancelling() {
+                    println!("Goal cancelled. quitting");
+                    let result_msg = Fibonacci::Result {
+                        sequence: feedback_msg.sequence,
+                    };
+                    g.cancel(result_msg).expect("could not send cancel request");
+                    // signal stopping of the node
+                    *done_cb.lock().unwrap() = true;
+                    return;
+                }
+                feedback_msg.sequence.push(feedback_msg.sequence[i] + feedback_msg.sequence[i - 1]);
+                g.publish_feedback(feedback_msg.clone()).expect("fail");
+                println!("Sending feedback: {:?}", feedback_msg);
+
+                timer.tick().await.unwrap();
+            }
+            let result_msg = Fibonacci::Result {
+                sequence: feedback_msg.sequence
+            };
+            g.succeed(result_msg).expect("could not set result");
+            // signal stopping of the node
+            *done_cb.lock().unwrap() = true;
+        }).expect("could not spawn task");
+    };
+
+    let _server = node.lock().unwrap().create_action_server::<Fibonacci::Action>("/fibonacci",
+                                                                                 Box::new(accept_goal_cb),
+                                                                                 Box::new(accept_cancel_cb),
+                                                                                 Box::new(handle_goal_cb))?;
+    loop {
+        node.lock().unwrap().spin_once(std::time::Duration::from_millis(100));
+        pool.run_until_stalled();
+        if *done.lock().unwrap() {
+            break;
+        }
+    }
+
+    Ok(())
+}

msg_gen/src/lib.rs

@@ -105,6 +105,7 @@ fn field_name(field_name: &str) -> String {
 pub fn generate_rust_service(module_: &str, prefix_: &str, name_: &str) -> String {
     format!(
         "
+        #[derive(Clone,Debug,PartialEq,Serialize,Deserialize)]
         pub struct Service();
         impl WrappedServiceTypeSupport for Service {{
             type Request = Request;
@@ -124,6 +125,7 @@ pub fn generate_rust_service(module_: &str, prefix_: &str, name_: &str) -> Strin
 pub fn generate_rust_action(module_: &str, prefix_: &str, name_: &str) -> String {
     format!(
         "
+        #[derive(Clone,Debug,PartialEq,Serialize,Deserialize)]
         pub struct Action();
         impl WrappedActionTypeSupport for Action {{
             type Goal = Goal;
@@ -148,12 +150,37 @@ pub fn generate_rust_action(module_: &str, prefix_: &str, name_: &str) -> String
                 }}
             }}
 
+            fn make_goal_response_msg(accepted: bool, stamp: builtin_interfaces::msg::Time) -> SendGoal::Response {{
+                SendGoal::Response {{
+                     accepted,
+                     stamp
+                }}
+            }}
+
+            fn make_feedback_msg(goal_id: unique_identifier_msgs::msg::UUID, feedback: Feedback) -> FeedbackMessage {{
+                FeedbackMessage {{
+                     goal_id,
+                     feedback
+                }}
+            }}
+
             fn make_result_request_msg(goal_id: unique_identifier_msgs::msg::UUID) -> GetResult::Request {{
                 GetResult::Request {{
                      goal_id,
                 }}
             }}
 
+            fn make_result_response_msg(status: i8, result: Result) -> GetResult::Response {{
+                GetResult::Response {{
+                     status,
+                     result,
+                }}
+            }}
+
+            fn destructure_goal_request_msg(msg: SendGoal::Request) -> (unique_identifier_msgs::msg::UUID, Goal) {{
+                (msg.goal_id, msg.goal)
+            }}
+
             fn destructure_goal_response_msg(msg: SendGoal::Response) -> (bool, builtin_interfaces::msg::Time) {{
                 (msg.accepted, msg.stamp)
             }}
@@ -165,6 +192,10 @@ pub fn generate_rust_action(module_: &str, prefix_: &str, name_: &str) -> String
             fn destructure_result_response_msg(msg: GetResult::Response) -> (i8, Result) {{
                 (msg.status, msg.result)
             }}
+
+            fn destructure_result_request_msg(msg: GetResult::Request) -> unique_identifier_msgs::msg::UUID {{
+                msg.goal_id
+            }}
         }}
 
             ",