From 9e6f572c7073428d31074427e04b1f9c4d4c2dc9 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 28 Sep 2024 15:02:03 -0400 Subject: [PATCH] Implement client-side trait methods for action messages This adds methods to ActionImpl for creating and accessing action-specific message types. These are needed by the rclrs ActionClient to generically read and write RMW messages. Due to issues with qualified paths in certain places (https://github.com/rust-lang/rust/issues/86935), the generator now refers directly to its service and message types rather than going through associated types of the various traits. This also makes the generated code a little easier to read, with the trait method signatures from rosidl_runtime_rs still enforcing type-safety. --- rclrs/src/action/server.rs | 12 ++-- rosidl_generator_rs/resource/action.rs.em | 67 +++++++++++++++++++---- rosidl_runtime_rs/src/traits.rs | 34 ++++++++++-- 3 files changed, 90 insertions(+), 23 deletions(-) diff --git a/rclrs/src/action/server.rs b/rclrs/src/action/server.rs index dfb4e965..0b035722 100644 --- a/rclrs/src/action/server.rs +++ b/rclrs/src/action/server.rs @@ -197,9 +197,7 @@ where mut request_id: rmw_request_id_t, accepted: bool, ) -> Result<(), RclrsError> { - type RmwResponse = <<::SendGoalService as Service>::Response as Message>::RmwMsg; - let mut response_rmw = RmwResponse::::default(); - ::set_goal_response_accepted(&mut response_rmw, accepted); + let mut response_rmw = ::create_goal_response(accepted, (0, 0)); let handle = &*self.handle.lock(); let result = unsafe { // SAFETY: The action server handle is locked and so synchronized with other @@ -210,7 +208,7 @@ where rcl_action_send_goal_response( handle, &mut request_id, - &mut response_rmw as *mut RmwResponse as *mut _, + &mut response_rmw as *mut _ as *mut _, ) } .ok(); @@ -242,7 +240,7 @@ where Err(err) => return Err(err), }; - let uuid = GoalUuid(::get_goal_request_uuid(&request)); + let uuid = GoalUuid(*::get_goal_request_uuid(&request)); let response: GoalResponse = { todo!("Optionally convert request to an idiomatic type for the user's callback."); @@ -522,7 +520,7 @@ where Err(err) => return Err(err), }; - let uuid = GoalUuid(::get_result_request_uuid(&request)); + let uuid = GoalUuid(*::get_result_request_uuid(&request)); let goal_exists = unsafe { // SAFETY: No preconditions @@ -640,7 +638,7 @@ where pub(crate) fn publish_feedback(&self, goal_id: &GoalUuid, feedback: &::Feedback) -> Result<(), RclrsError> { let feedback_rmw = <::Feedback as Message>::into_rmw_message(std::borrow::Cow::Borrowed(feedback)); - let mut feedback_msg = ::create_feedback_message(&goal_id.0, &*feedback_rmw); + let mut feedback_msg = ::create_feedback_message(&goal_id.0, feedback_rmw.into_owned()); unsafe { // SAFETY: The action server is locked through the handle, meaning that no other // non-thread-safe functions can be called on it at the same time. The feedback_msg is diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em index 3257d44b..4c72b90b 100644 --- a/rosidl_generator_rs/resource/action.rs.em +++ b/rosidl_generator_rs/resource/action.rs.em @@ -6,6 +6,8 @@ from rosidl_parser.definition import ( ACTION_GOAL_SUFFIX, ACTION_RESULT_SERVICE_SUFFIX, ACTION_RESULT_SUFFIX, + SERVICE_REQUEST_MESSAGE_SUFFIX, + SERVICE_RESPONSE_MESSAGE_SUFFIX, ) action_msg_specs = [] @@ -100,31 +102,74 @@ impl rosidl_runtime_rs::ActionImpl for @(type_name) { type CancelGoalService = action_msgs::srv::rmw::CancelGoal; type GetResultService = crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX); - fn get_goal_request_uuid(request: &<::Request as rosidl_runtime_rs::Message>::RmwMsg) -> [u8; 16] { - request.goal_id.uuid + fn create_goal_request(goal_id: &[u8; 16], goal: crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SUFFIX)) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX) { + crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX) { + goal_id: unique_identifier_msgs::msg::rmw::UUID { uuid: *goal_id }, + goal, + } + } + + fn get_goal_request_uuid(request: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX)) -> &[u8; 16] { + &request.goal_id.uuid + } + + fn create_goal_response(accepted: bool, stamp: (i32, u32)) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX) { + crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX) { + accepted, + stamp: builtin_interfaces::msg::rmw::Time { + sec: stamp.0, + nanosec: stamp.1, + }, + } + } + + fn get_goal_response_accepted(response: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX)) -> bool { + response.accepted } - fn set_goal_response_accepted(response: &mut <::Response as rosidl_runtime_rs::Message>::RmwMsg, accepted: bool) { - response.accepted = accepted; + fn get_goal_response_stamp(response: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX)) -> (i32, u32) { + (response.stamp.sec, response.stamp.nanosec) } - fn create_feedback_message(goal_id: &[u8; 16], feedback: &<::Feedback as rosidl_runtime_rs::Message>::RmwMsg) -> ::RmwMsg { - let mut message = ::RmwMsg::default(); + fn create_feedback_message(goal_id: &[u8; 16], feedback: crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_SUFFIX)) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX) { + let mut message = crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX)::default(); message.goal_id.uuid = *goal_id; - message.feedback = feedback.clone(); + message.feedback = feedback; message } - fn get_result_request_uuid(request: &<::Request as rosidl_runtime_rs::Message>::RmwMsg) -> [u8; 16] { - request.goal_id.uuid + fn get_feedback_message_uuid(feedback: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX)) -> &[u8; 16] { + &feedback.goal_id.uuid + } + + fn get_feedback_message_feedback(feedback: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX)) -> &crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_SUFFIX) { + &feedback.feedback } - fn create_result_response(status: i8, result: <::Result as Message>::RmwMsg) -> <::Response as Message>::RmwMsg { - <::Response as rosidl_runtime_rs::Message>::RmwMsg { + fn create_result_request(goal_id: &[u8; 16]) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX) { + crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX) { + goal_id: unique_identifier_msgs::msg::rmw::UUID { uuid: *goal_id }, + } + } + + fn get_result_request_uuid(request: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_REQUEST_MESSAGE_SUFFIX)) -> &[u8; 16] { + &request.goal_id.uuid + } + + fn create_result_response(status: i8, result: crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SUFFIX)) -> crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX) { + crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX) { status, result, } } + + fn get_result_response_result(response: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX)) -> &crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SUFFIX) { + &response.result + } + + fn get_result_response_status(response: &crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX)@(SERVICE_RESPONSE_MESSAGE_SUFFIX)) -> i8 { + response.status + } } @[end for] diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index 4ac60e51..d0402dd5 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -196,18 +196,42 @@ pub trait ActionImpl: 'static + Action { /// The get_result service associated with this action. type GetResultService: Service; + /// Create a goal request message with the given UUID and goal. + fn create_goal_request(goal_id: &[u8; 16], goal: <::Goal as Message>::RmwMsg) -> <::Request as Message>::RmwMsg; + /// Get the UUID of a goal request. - fn get_goal_request_uuid(request: &<::Request as Message>::RmwMsg) -> [u8; 16]; + fn get_goal_request_uuid(request: &<::Request as Message>::RmwMsg) -> &[u8; 16]; + + /// Create a goal response message with the given acceptance and timestamp. + fn create_goal_response(accepted: bool, stamp: (i32, u32)) -> <::Response as Message>::RmwMsg; - /// Sets the `accepted` field of a goal response. - fn set_goal_response_accepted(response: &mut <::Response as Message>::RmwMsg, accepted: bool); + /// Get the `accepted` field of a goal response. + fn get_goal_response_accepted(response: &<::Response as Message>::RmwMsg) -> bool; + + /// Get the `stamp` field of a goal response. + fn get_goal_response_stamp(response: &<::Response as Message>::RmwMsg) -> (i32, u32); /// Create a feedback message with the given goal ID and contents. - fn create_feedback_message(goal_id: &[u8; 16], feedback: &<::Feedback as Message>::RmwMsg) -> ::RmwMsg; + fn create_feedback_message(goal_id: &[u8; 16], feedback: <::Feedback as Message>::RmwMsg) -> ::RmwMsg; + + /// Get the UUID of a feedback message. + fn get_feedback_message_uuid(feedback: &::RmwMsg) -> &[u8; 16]; + + /// Get the feedback of a feedback message. + fn get_feedback_message_feedback(feedback: &::RmwMsg) -> &<::Feedback as Message>::RmwMsg; + + /// Create a result request message with the given goal ID. + fn create_result_request(goal_id: &[u8; 16]) -> <::Request as Message>::RmwMsg; /// Get the UUID of a result request. - fn get_result_request_uuid(request: &<::Request as Message>::RmwMsg) -> [u8; 16]; + fn get_result_request_uuid(request: &<::Request as Message>::RmwMsg) -> &[u8; 16]; /// Create a result response message with the given status and contents. fn create_result_response(status: i8, result: <::Result as Message>::RmwMsg) -> <::Response as Message>::RmwMsg; + + /// Get the result of a result response. + fn get_result_response_result(response: &<::Response as Message>::RmwMsg) -> &<::Result as Message>::RmwMsg; + + /// Get the status of a result response. + fn get_result_response_status(response: &<::Response as Message>::RmwMsg) -> i8; }