checkin

Author: esteve

examples/minimal_action_server/src/minimal_action_server.rs

@@ -1,11 +1,51 @@
 use std::env;
+use std::sync::Arc;
+use std::thread;
 
 use anyhow::{Error, Result};
 
+type Fibonacci = example_interfaces::action::Fibonacci;
+type GoalHandleFibonacci = rclrs::ServerGoalHandle<Fibonacci>;
+
+fn handle_goal(
+    _uuid: &rclrs::GoalUUID,
+    goal: Arc<example_interfaces::action::rmw::Fibonacci_Goal>,
+) -> rclrs::GoalResponse {
+    println!("Received goal request with order {}", goal.order);
+    if goal.order > 9000 {
+        rclrs::GoalResponse::Reject
+    } else {
+        rclrs::GoalResponse::AcceptAndExecute
+    }
+}
+
+fn handle_cancel(_goal_handle: Arc<GoalHandleFibonacci>) -> rclrs::CancelResponse {
+    println!("Got request to cancel goal");
+    rclrs::CancelResponse::Accept
+}
+
+fn execute(goal_handle: Arc<GoalHandleFibonacci>) {
+    println!("Executing goal");
+    thread::sleep(std::time::Duration::from_millis(100));
+}
+
+fn handle_accepted(goal_handle: Arc<GoalHandleFibonacci>) {
+    thread::spawn(move || {
+        execute(goal_handle);
+    });
+}
+
 fn main() -> Result<(), Error> {
     let context = rclrs::Context::new(env::args())?;
 
-    let node = rclrs::create_node(&context, "minimal_action_server")?;
+    let mut node = rclrs::create_node(&context, "minimal_action_server")?;
+
+    let _action_server = node.create_action_server::<example_interfaces::action::Fibonacci>(
+        "fibonacci",
+        handle_goal,
+        handle_cancel,
+        handle_accepted,
+    );
 
     rclrs::spin(&node).map_err(|err| err.into())
 }

rclrs/src/action.rs

@@ -1,8 +1,27 @@
 use crate::{rcl_bindings::*, RclrsError};
 use std::sync::{Arc, Mutex, MutexGuard};
 
+// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
+// they are running in. Therefore, this type can be safely sent to another thread.
+unsafe impl Send for rcl_action_goal_handle_t {}
+
+unsafe impl Sync for rcl_action_goal_handle_t {}
+
 use std::marker::PhantomData;
 
+pub type GoalUUID = [u8; RCL_ACTION_UUID_SIZE];
+
+pub enum GoalResponse {
+    Reject = 1,
+    AcceptAndExecute = 2,
+    AcceptAndDefer = 3,
+}
+
+pub enum CancelResponse {
+    Reject = 1,
+    Accept = 2,
+}
+
 pub struct ActionClient<T>
 where
     T: rosidl_runtime_rs::Action,
@@ -16,8 +35,6 @@ where
 {
     /// Creates a new action client.
     pub(crate) fn new(rcl_node_mtx: Arc<Mutex<rcl_node_t>>, topic: &str) -> Result<Self, RclrsError>
-    // This uses pub(crate) visibility to avoid instantiating this struct outside
-    // [`Node::create_client`], see the struct's documentation for the rationale
     where
         T: rosidl_runtime_rs::Action,
     {
@@ -26,3 +43,52 @@ where
         })
     }
 }
+
+pub struct ActionServer<T>
+where
+    T: rosidl_runtime_rs::Action,
+{
+    _marker: PhantomData<T>,
+}
+
+impl<T> ActionServer<T>
+where
+    T: rosidl_runtime_rs::Action,
+{
+    /// Creates a new action server.
+    pub(crate) fn new(rcl_node_mtx: Arc<Mutex<rcl_node_t>>, topic: &str) -> Result<Self, RclrsError>
+    where
+        T: rosidl_runtime_rs::Action,
+    {
+        Ok(Self {
+            _marker: Default::default(),
+        })
+    }
+}
+
+pub struct ServerGoalHandle<T>
+where
+    T: rosidl_runtime_rs::Action,
+{
+    rcl_handle: Arc<rcl_action_goal_handle_t>,
+    _marker: PhantomData<T>,
+}
+
+impl<T> ServerGoalHandle<T>
+where
+    T: rosidl_runtime_rs::Action,
+{
+    pub(crate) fn new(rcl_handle: Arc<rcl_action_goal_handle_t>) {}
+
+    pub(crate) fn is_canceling() -> bool {
+        false
+    }
+
+    pub(crate) fn is_active() -> bool {
+        false
+    }
+
+    pub(crate) fn is_executing() -> bool {
+        false
+    }
+}

rclrs/src/lib.rs

@@ -42,6 +42,8 @@ pub use node::*;
 pub use parameter::*;
 pub use publisher::*;
 pub use qos::*;
+use rcl_bindings::rcl_context_is_valid;
+use rcl_bindings::rcl_action_goal_handle_t;
 pub use rcl_bindings::rmw_request_id_t;
 pub use service::*;
 pub use subscription::*;

rclrs/src/node.rs

@@ -13,9 +13,10 @@ pub use self::builder::*;
 pub use self::graph::*;
 use crate::rcl_bindings::*;
 use crate::{
-    ActionClient, Client, ClientBase, Clock, Context, GuardCondition, ParameterBuilder,
-    ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service,
-    ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, TimeSource, ToResult,
+    ActionClient, ActionServer, CancelResponse, Client, ClientBase, Clock, Context, GoalResponse,
+    GoalUUID, GuardCondition, ParameterInterface, ParameterVariant, Parameters, Publisher,
+    QoSProfile, RclrsError, ServerGoalHandle, Service, ServiceBase, Subscription, SubscriptionBase,
+    SubscriptionCallback, TimeSource, ToResult,
 };
 
 impl Drop for rcl_node_t {
@@ -195,7 +196,7 @@ impl Node {
         Ok(client)
     }
 
-    /// Creates a [`Client`][1].
+    /// Creates an [`ActionClient`][1].
     ///
     /// [1]: crate::ActionClient
     // TODO: make action client's lifetime depend on node's lifetime
@@ -206,13 +207,36 @@ impl Node {
     where
         T: rosidl_runtime_rs::Action,
     {
-        let client = Arc::new(ActionClient::<T>::new(
+        let action_client = Arc::new(ActionClient::<T>::new(
             Arc::clone(&self.rcl_node_mtx),
             topic,
         )?);
         // self.clients
         //     .push(Arc::downgrade(&client) as Weak<dyn ClientBase>);
-        Ok(client)
+        Ok(action_client)
+    }
+
+    /// Creates an [`ActionServer`][1].
+    ///
+    /// [1]: crate::ActionServer
+    // TODO: make action server's lifetime depend on node's lifetime
+    pub fn create_action_server<T>(
+        &mut self,
+        topic: &str,
+        handle_goal: fn(&crate::action::GoalUUID, Arc<T::Goal>) -> GoalResponse,
+        handle_cancel: fn(Arc<ServerGoalHandle<T>>) -> CancelResponse,
+        handle_accepted: fn(Arc<ServerGoalHandle<T>>),
+    ) -> Result<Arc<ActionServer<T>>, RclrsError>
+    where
+        T: rosidl_runtime_rs::Action,
+    {
+        let action_server = Arc::new(ActionServer::<T>::new(
+            Arc::clone(&self.rcl_node_mtx),
+            topic,
+        )?);
+        // self.servers
+        //     .push(Arc::downgrade(&server) as Weak<dyn ClientBase>);
+        Ok(action_server)
     }
 
     /// Creates a [`GuardCondition`][1] with no callback.

rclrs/src/rcl_bindings.rs

@@ -14,3 +14,4 @@
 include!(concat!(env!("OUT_DIR"), "/rcl_bindings_generated.rs"));
 
 pub const RMW_GID_STORAGE_SIZE: usize = rmw_gid_storage_size_constant;
+pub const RCL_ACTION_UUID_SIZE: usize = rcl_action_uuid_size_constant;

rclrs/src/rcl_wrapper.h

@@ -1,9 +1,12 @@
 #include <rcl/graph.h>
 #include <rcl/rcl.h>
+#include <rcl_action/types.h>
+#include <rcl_action/goal_handle.h>
 #include <rcl_yaml_param_parser/parser.h>
 #include <rcutils/error_handling.h>
 #include <rmw/types.h>
 #include <rosidl_typesupport_introspection_c/field_types.h>
 #include <rosidl_typesupport_introspection_c/message_introspection.h>
 
 const size_t rmw_gid_storage_size_constant = RMW_GID_STORAGE_SIZE;
+const size_t rcl_action_uuid_size_constant = UUID_SIZE;