From 06ee9bf5952af09786f4c4d36316773b6712d810 Mon Sep 17 00:00:00 2001 From: alluring-mushroom <86041465+alluring-mushroom@users.noreply.github.com> Date: Wed, 9 Oct 2024 18:05:25 +1000 Subject: [PATCH 01/20] Add std::error::Error impls to Error enums of `parameter` module (#413) --- rclrs/src/parameter.rs | 33 +++++++++++++++++++++++++++++++++ rclrs/src/parameter/value.rs | 13 +++++++++++++ 2 files changed, 46 insertions(+) diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index c8a710eeb..f8427a0cc 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -624,6 +624,18 @@ pub enum ParameterValueError { ReadOnly, } +impl std::fmt::Display for ParameterValueError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + ParameterValueError::OutOfRange => write!(f, "parameter value was out of the parameter's range"), + ParameterValueError::TypeMismatch => write!(f, "parameter was stored in a static type and an operation on a different type was attempted"), + ParameterValueError::ReadOnly => write!(f, "a write on a read-only parameter was attempted"), + } + } +} + +impl std::error::Error for ParameterValueError {} + /// Error that can be generated when doing operations on parameters. #[derive(Debug)] pub enum DeclarationError { @@ -644,6 +656,27 @@ pub enum DeclarationError { InvalidRange, } +impl std::fmt::Display for DeclarationError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + DeclarationError::AlreadyDeclared => write!(f, "parameter was already declared, but a new declaration was attempted"), + DeclarationError::NoValueAvailable => { + write!(f, "parameter was declared as non-optional but no value was available, either through a user specified default, a command-line override, or a previously set value") + }, + DeclarationError::OverrideValueTypeMismatch => { + write!(f, "the override value that was provided has the wrong type") + }, + DeclarationError::PriorValueTypeMismatch => { + write!(f, "the value that the parameter was already set to has the wrong type") + }, + DeclarationError::InitialValueOutOfRange => write!(f, "the initial value that was selected is out of range"), + DeclarationError::InvalidRange => write!(f, "an invalid range was provided to a parameter declaration (i.e. lower bound > higher bound)"), + } + } +} + +impl std::error::Error for DeclarationError {} + impl<'a> Parameters<'a> { /// Tries to read a parameter of the requested type. /// diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 338654994..82fe31ebb 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -376,11 +376,24 @@ impl From for RmwParameterValue { /// An error that occured when trying to convert a parameter from an /// `rcl_interfaces::msg::ParameterValue` +#[derive(Debug)] pub enum RmwParameterConversionError { /// The parameter type was not valid. InvalidParameterType, } +impl std::fmt::Display for RmwParameterConversionError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + RmwParameterConversionError::InvalidParameterType => { + write!(f, "the parameter type was not valid") + } + } + } +} + +impl std::error::Error for RmwParameterConversionError {} + impl TryFrom for ParameterValue { type Error = RmwParameterConversionError; From dad5a511ecb9999e070b2110a2a0af19ff67ac24 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Sat, 26 Oct 2024 14:11:57 -0400 Subject: [PATCH 02/20] Action message support (#417) * Added action template * Added action generation * Added basic create_action_client function * dded action generation * checkin * Fix missing exported pre_field_serde field * Removed extra code * Sketch out action server construction and destruction This follows generally the same pattern as the service server. It required adding a typesupport function to the Action trait and pulling in some more rcl_action bindings. * Fix action typesupport function * Add ActionImpl trait with internal messages and services This is incomplete, since the service types aren't yet being generated. * Split srv.rs.em into idiomatic and rmw template files This results in the exact same file being produced for services, except for some whitespace changes. However, it enables actions to invoke the respective service template for its generation, similar to how the it works for services and their underlying messages. * Generate underlying service definitions for actions Not tested * Add runtime trait to get the UUID from a goal request C++ uses duck typing for this, knowing that for any `Action`, the type `Action::Impl::SendGoalService::Request` will always have a `goal_id` field of type `unique_identifier_msgs::msg::UUID` without having to prove this to the compiler. Rust's generics are more strict, requiring that this be proven using type bounds. The `Request` type is also action-specific as it contains a `goal` field containing the `Goal` message type of the action. We therefore cannot enforce that all `Request`s are a specific type in `rclrs`. This seems most easily represented using associated type bounds on the `SendGoalService` associated type within `ActionImpl`. To avoid introducing to `rosidl_runtime_rs` a circular dependency on message packages like `unique_identifier_msgs`, the `ExtractUuid` trait only operates on a byte array rather than a more nicely typed `UUID` message type. I'll likely revisit this as we introduce more similar bounds on the generated types. * Integrate RMW message methods into ActionImpl Rather than having a bunch of standalone traits implementing various message functions like `ExtractUuid` and `SetAccepted`, with the trait bounds on each associated type in `ActionImpl`, we'll instead add these functions directly to the `ActionImpl` trait. This is simpler on both the rosidl_runtime_rs and the rclrs side. * Add rosidl_runtime_rs::ActionImpl::create_feedback_message() Adds a trait method to create a feedback message given the goal ID and the user-facing feedback message type. Depending on how many times we do this, it may end up valuable to define a GoalUuid type in rosidl_runtime_rs itself. We wouldn't be able to utilize the `RCL_ACTION_UUID_SIZE` constant imported from `rcl_action`, but this is pretty much guaranteed to be 16 forever. Defining this method signature also required inverting the super-trait relationship between Action and ActionImpl. Now ActionImpl is the sub-trait as this gives it access to all of Action's associated types. Action doesn't need to care about anything from ActionImpl (hopefully). * Add GetResultService methods to ActionImpl * Implement ActionImpl trait methods in generator These still don't build without errors, but it's close. * Replace set_result_response_status with create_result_response rclrs needs to be able to generically construct result responses, including the user-defined result field. * 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. * Format the rosidl_runtime_rs::ActionImpl trait * Wrap longs lines in rosidl_generator_rs action.rs This at least makes the template easier to read, but also helps with the generated code. In general, the generated code could actually fit on one line for the function signatures, but it's not a big deal to split it across multiple. * Use idiomatic message types in Action trait This is user-facing and so should use the friendly message types. * Cleanup ActionImpl using type aliases Signed-off-by: Michael X. Grey * Formatting * Switch from std::os::raw::c_void to std::ffi::c_void While these are aliases of each other, we might as well use the more appropriate std::ffi version, as requested by reviewers. * Clean up rosidl_generator_rs's cmake files Some of the variables are present but no longer used. Others were not updated with the action changes. * Add a short doc page on the message generation pipeline This should help newcomers orient themselves around the rosidl_*_rs packages. --------- Signed-off-by: Michael X. Grey Co-authored-by: Esteve Fernandez Co-authored-by: Michael X. Grey --- docs/message-generation.md | 49 +++++ .../cmake/custom_command.cmake | 8 +- ...idl_generator_rs_generate_interfaces.cmake | 25 ++- rosidl_generator_rs/resource/action.rs.em | 205 ++++++++++++++++++ rosidl_generator_rs/resource/lib.rs.em | 4 + rosidl_generator_rs/resource/msg_rmw.rs.em | 4 +- rosidl_generator_rs/resource/srv.rs.em | 69 +----- .../resource/srv_idiomatic.rs.em | 44 ++++ rosidl_generator_rs/resource/srv_rmw.rs.em | 44 ++++ .../rosidl_generator_rs/__init__.py | 27 +++ rosidl_runtime_rs/src/lib.rs | 2 +- rosidl_runtime_rs/src/traits.rs | 96 +++++++- 12 files changed, 495 insertions(+), 82 deletions(-) create mode 100644 docs/message-generation.md create mode 100644 rosidl_generator_rs/resource/action.rs.em create mode 100644 rosidl_generator_rs/resource/srv_idiomatic.rs.em create mode 100644 rosidl_generator_rs/resource/srv_rmw.rs.em diff --git a/docs/message-generation.md b/docs/message-generation.md new file mode 100644 index 000000000..ea28d57ed --- /dev/null +++ b/docs/message-generation.md @@ -0,0 +1,49 @@ +# `ros2_rust` Message Generation + +The `ros2_rust` project strives to maintain consistency with the upstream ROS 2 message generation +system. To this end, it provides two main packages: `rosidl_generator_rs` and `rosidl_runtime_rs`. +These packages provide the infrastructure required for defining and using ROS 2 messages, services, +and actions in Rust. + +At a high level, the `rosidl_generator_rs` package handles generation of interface crates from the +`.msg`, `.srv`, and `.action` files defined by a user. The `rosidl_runtime_rs` package provides +common functionality shared across message packages, such as support types and traits. Each of these +packages is described in more detail below. + +## `rosidl_generator_rs` + +`rosidl_generator_rs` follows a very similar pattern to the other message generation packages for +ROS 2. To tie into this pipeline, it registers itself as a `"rosidl_generate_idl_interfaces"` +extension with `ament`. Doing so ensures that message packages calling `rosidl_generate_interfaces` +will invoke the Rust language generator in addition to any others. This is accomplished using the +various CMake scripts under the `cmake` directory. When this happens, the input interface files will +be converted into IDL files which, along with additional metadata, are fed into the `generate_rs` +function of `rosidl_generator_rs/__init__.py`. + +From here, the IDL files are parsed into an internal representation using the upstream +[`rosidl_parser`](https://github.com/ros2/rosidl/tree/rolling/rosidl_parser) package. This abstract +representation is then used to instantiate the various template files under the `resource` +subdirectory, producing a full Rust crate for each package. + +For each input message type, two `struct`s are generated: + +- An ergonomic representation of the message, using more idiomatic `std` types like `Vec` and + `String` for sequence and string fields. These are placed directly in the `msg` submodule of the + crate. +- A FFI-suitable `struct` that is ABI-compatible with the layout expected by the RMW layer. While + less ergonomic, these avoid the conversion overhead when passed to RMW. These `struct`s are placed + under the `msg::rmw` submodule. + +All the produced `struct`s implement the standard traits from `std` when possible, such as `Clone`, +`PartialEq`, and `Debug`. Additionally, when the generated crate's `serde` feature is enabled, these +structs implement the `Serialize` and `Deserialize` traits. + +## `rosidl_runtime_rs` + +`rosidl_runtime_rs` is a runtime support package, providing `Message`, `Service`, and `Action` +traits that are implemented by the corresponding structs generated by `rosidl_generator_rs`. These +allow for generic interaction with these various interface types, both in client libraries like +`rclrs` and in user code. + +The package also provides a number of string and sequence types that are ABI-compatible with their +`rosidl_runtime_c` equivalents. This allows for more ergonomic use of the RMW-native message types. diff --git a/rosidl_generator_rs/cmake/custom_command.cmake b/rosidl_generator_rs/cmake/custom_command.cmake index a3f3ff916..d16ccd863 100644 --- a/rosidl_generator_rs/cmake/custom_command.cmake +++ b/rosidl_generator_rs/cmake/custom_command.cmake @@ -14,12 +14,10 @@ add_custom_command( OUTPUT - ${_generated_extension_files} ${_generated_common_rs_files} ${_generated_msg_rs_files} - ${_generated_msg_c_files} ${_generated_srv_rs_files} - ${_generated_srv_c_files} + ${_generated_action_rs_files} COMMAND ${PYTHON_EXECUTABLE} ${rosidl_generator_rs_BIN} --generator-arguments-file "${generator_arguments_file}" --typesupport-impls "${_typesupport_impls}" @@ -34,11 +32,9 @@ else() add_custom_target( ${rosidl_generate_interfaces_TARGET}${_target_suffix} ALL DEPENDS - ${_generated_extension_files} ${_generated_common_rs_files} ${_generated_msg_rs_files} - ${_generated_msg_c_files} ${_generated_srv_rs_files} - ${_generated_srv_c_files} + ${_generated_action_rs_files} ) endif() diff --git a/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake b/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake index 9cfdfa579..af4206161 100644 --- a/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake +++ b/rosidl_generator_rs/cmake/rosidl_generator_rs_generate_interfaces.cmake @@ -26,9 +26,11 @@ set(_generated_common_rs_files "") set(_generated_msg_rs_files "") set(_generated_srv_rs_files "") +set(_generated_action_rs_files "") set(_has_msg FALSE) set(_has_srv FALSE) +set(_has_action FALSE) foreach(_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) get_filename_component(_parent_folder "${_idl_file}" DIRECTORY) @@ -37,13 +39,13 @@ foreach(_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) if(_parent_folder STREQUAL "msg") set(_has_msg TRUE) - set(_idl_file_without_actions ${_idl_file_without_actions} ${_idl_file}) + set(_idl_files ${_idl_files} ${_idl_file}) elseif(_parent_folder STREQUAL "srv") set(_has_srv TRUE) - set(_idl_file_without_actions ${_idl_file_without_actions} ${_idl_file}) + set(_idl_files ${_idl_files} ${_idl_file}) elseif(_parent_folder STREQUAL "action") set(_has_action TRUE) - message(WARNING "Rust actions generation is not implemented") + set(_idl_files ${_idl_files} ${_idl_file}) else() message(FATAL_ERROR "Interface file with unknown parent folder: ${_idl_file}") endif() @@ -67,6 +69,12 @@ if(${_has_srv}) ) endif() +if(${_has_action}) + list(APPEND _generated_action_rs_files + "${_output_path}/rust/src/action.rs" + ) +endif() + set(_dependency_files "") set(_dependencies "") foreach(_pkg_name ${rosidl_generate_interfaces_DEPENDENCY_PACKAGE_NAMES}) @@ -81,12 +89,15 @@ endforeach() set(target_dependencies "${rosidl_generator_rs_BIN}" ${rosidl_generator_rs_GENERATOR_FILES} + "${rosidl_generator_rs_TEMPLATE_DIR}/action.rs.em" "${rosidl_generator_rs_TEMPLATE_DIR}/msg_idiomatic.rs.em" "${rosidl_generator_rs_TEMPLATE_DIR}/msg_rmw.rs.em" "${rosidl_generator_rs_TEMPLATE_DIR}/msg.rs.em" + "${rosidl_generator_rs_TEMPLATE_DIR}/srv_idiomatic.rs.em" + "${rosidl_generator_rs_TEMPLATE_DIR}/srv_rmw.rs.em" "${rosidl_generator_rs_TEMPLATE_DIR}/srv.rs.em" ${rosidl_generate_interfaces_ABS_IDL_FILES} - ${_idl_file_without_actions} + ${_idl_files} ${_dependency_files}) foreach(dep ${target_dependencies}) if(NOT EXISTS "${dep}") @@ -99,7 +110,7 @@ rosidl_write_generator_arguments( "${generator_arguments_file}" PACKAGE_NAME "${PROJECT_NAME}" IDL_TUPLES "${rosidl_generate_interfaces_IDL_TUPLES}" - ROS_INTERFACE_FILES "${_idl_file_without_actions}" + ROS_INTERFACE_FILES "${_idl_files}" ROS_INTERFACE_DEPENDENCIES "${_dependencies}" OUTPUT_DIR "${_output_path}" TEMPLATE_DIR "${rosidl_generator_rs_TEMPLATE_DIR}" @@ -130,6 +141,7 @@ set_property( ${_generated_common_rs_files} ${_generated_msg_rs_files} ${_generated_srv_rs_files} + ${_generated_action_rs_files} PROPERTY GENERATED 1) set(_rsext_suffix "__rsext") @@ -144,7 +156,8 @@ endif() if(BUILD_TESTING AND rosidl_generate_interfaces_ADD_LINTER_TESTS) if( NOT _generated_msg_rs_files STREQUAL "" OR - NOT _generated_srv_rs_files STREQUAL "" + NOT _generated_srv_rs_files STREQUAL "" OR + NOT _generated_action_rs_files STREQUAL "" ) # TODO(esteve): add linters for Rust files endif() diff --git a/rosidl_generator_rs/resource/action.rs.em b/rosidl_generator_rs/resource/action.rs.em new file mode 100644 index 000000000..5f463f318 --- /dev/null +++ b/rosidl_generator_rs/resource/action.rs.em @@ -0,0 +1,205 @@ +@{ +from rosidl_parser.definition import ( + ACTION_FEEDBACK_MESSAGE_SUFFIX, + ACTION_FEEDBACK_SUFFIX, + ACTION_GOAL_SERVICE_SUFFIX, + ACTION_GOAL_SUFFIX, + ACTION_RESULT_SERVICE_SUFFIX, + ACTION_RESULT_SUFFIX, + SERVICE_REQUEST_MESSAGE_SUFFIX, + SERVICE_RESPONSE_MESSAGE_SUFFIX, +) + +action_msg_specs = [] + +for subfolder, action in action_specs: + action_msg_specs.append((subfolder, action.goal)) + action_msg_specs.append((subfolder, action.result)) + action_msg_specs.append((subfolder, action.feedback)) + action_msg_specs.append((subfolder, action.feedback_message)) + +action_srv_specs = [] + +for subfolder, action in action_specs: + action_srv_specs.append((subfolder, action.send_goal_service)) + action_srv_specs.append((subfolder, action.get_result_service)) +}@ + +pub mod rmw { +@{ +TEMPLATE( + 'msg_rmw.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=action_msg_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) + +TEMPLATE( + 'srv_rmw.rs.em', + package_name=package_name, interface_path=interface_path, + srv_specs=action_srv_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ +} // mod rmw + +@{ +TEMPLATE( + 'msg_idiomatic.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=action_msg_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ + +@{ +TEMPLATE( + 'srv_idiomatic.rs.em', + package_name=package_name, interface_path=interface_path, + srv_specs=action_srv_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ + +@[for subfolder, action_spec in action_specs] + +@{ +type_name = action_spec.namespaced_type.name +}@ + +#[link(name = "@(package_name)__rosidl_typesupport_c")] +extern "C" { + fn rosidl_typesupport_c__get_action_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::ffi::c_void; +} + +// Corresponds to @(package_name)__@(subfolder)__@(type_name) +pub struct @(type_name); + +impl rosidl_runtime_rs::Action for @(type_name) { + type Goal = crate::@(subfolder)::@(type_name)@(ACTION_GOAL_SUFFIX); + type Result = crate::@(subfolder)::@(type_name)@(ACTION_RESULT_SUFFIX); + type Feedback = crate::@(subfolder)::@(type_name)@(ACTION_FEEDBACK_SUFFIX); + + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { rosidl_typesupport_c__get_action_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } + } +} + +impl rosidl_runtime_rs::ActionImpl for @(type_name) { + type GoalStatusMessage = action_msgs::msg::rmw::GoalStatusArray; + type FeedbackMessage = crate::@(subfolder)::rmw::@(type_name)@(ACTION_FEEDBACK_MESSAGE_SUFFIX); + + type SendGoalService = crate::@(subfolder)::rmw::@(type_name)@(ACTION_GOAL_SERVICE_SUFFIX); + type CancelGoalService = action_msgs::srv::rmw::CancelGoal; + type GetResultService = crate::@(subfolder)::rmw::@(type_name)@(ACTION_RESULT_SERVICE_SUFFIX); + + 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 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: 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; + message + } + + 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_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_generator_rs/resource/lib.rs.em b/rosidl_generator_rs/resource/lib.rs.em index 51e4a5ba4..79a0e1def 100644 --- a/rosidl_generator_rs/resource/lib.rs.em +++ b/rosidl_generator_rs/resource/lib.rs.em @@ -7,3 +7,7 @@ pub mod msg; @[if len(srv_specs) > 0]@ pub mod srv; @[end if]@ + +@[if len(action_specs) > 0]@ +pub mod action; +@[end if]@ diff --git a/rosidl_generator_rs/resource/msg_rmw.rs.em b/rosidl_generator_rs/resource/msg_rmw.rs.em index c4420a685..fbedd6d5f 100644 --- a/rosidl_generator_rs/resource/msg_rmw.rs.em +++ b/rosidl_generator_rs/resource/msg_rmw.rs.em @@ -19,7 +19,7 @@ type_name = msg_spec.structure.namespaced_type.name #[link(name = "@(package_name)__rosidl_typesupport_c")] extern "C" { - fn rosidl_typesupport_c__get_message_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; + fn rosidl_typesupport_c__get_message_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::ffi::c_void; } #[link(name = "@(package_name)__rosidl_generator_c")] @@ -103,7 +103,7 @@ impl rosidl_runtime_rs::Message for @(type_name) { impl rosidl_runtime_rs::RmwMessage for @(type_name) where Self: Sized { const TYPE_NAME: &'static str = "@(package_name)/@(subfolder)/@(type_name)"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } } diff --git a/rosidl_generator_rs/resource/srv.rs.em b/rosidl_generator_rs/resource/srv.rs.em index 369696ff7..dd99e8e76 100644 --- a/rosidl_generator_rs/resource/srv.rs.em +++ b/rosidl_generator_rs/resource/srv.rs.em @@ -1,84 +1,23 @@ -@{ -req_res_specs = [] - -for subfolder, service in srv_specs: - req_res_specs.append((subfolder, service.request_message)) - req_res_specs.append((subfolder, service.response_message)) -}@ - @{ TEMPLATE( - 'msg_idiomatic.rs.em', + 'srv_idiomatic.rs.em', package_name=package_name, interface_path=interface_path, - msg_specs=req_res_specs, + srv_specs=srv_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) -}@ - -@[for subfolder, srv_spec in srv_specs] - -@{ -type_name = srv_spec.namespaced_type.name -}@ - -#[link(name = "@(package_name)__rosidl_typesupport_c")] -extern "C" { - fn rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; } -// Corresponds to @(package_name)__@(subfolder)__@(type_name) -pub struct @(type_name); - -impl rosidl_runtime_rs::Service for @(type_name) { - type Request = crate::@(subfolder)::@(type_name)_Request; - type Response = crate::@(subfolder)::@(type_name)_Response; - - fn get_type_support() -> *const std::os::raw::c_void { - // SAFETY: No preconditions for this function. - unsafe { rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } - } -} - -@[end for] - pub mod rmw { @{ TEMPLATE( - 'msg_rmw.rs.em', + 'srv_rmw.rs.em', package_name=package_name, interface_path=interface_path, - msg_specs=req_res_specs, + srv_specs=srv_specs, get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, pre_field_serde=pre_field_serde, get_idiomatic_rs_type=get_idiomatic_rs_type, constant_value_to_rs=constant_value_to_rs) }@ - -@[for subfolder, srv_spec in srv_specs] - -@{ -type_name = srv_spec.namespaced_type.name -}@ - - #[link(name = "@(package_name)__rosidl_typesupport_c")] - extern "C" { - fn rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::os::raw::c_void; - } - - // Corresponds to @(package_name)__@(subfolder)__@(type_name) - pub struct @(type_name); - - impl rosidl_runtime_rs::Service for @(type_name) { - type Request = crate::@(subfolder)::rmw::@(type_name)_Request; - type Response = crate::@(subfolder)::rmw::@(type_name)_Response; - - fn get_type_support() -> *const std::os::raw::c_void { - // SAFETY: No preconditions for this function. - unsafe { rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } - } - } - -@[end for] - } // mod rmw diff --git a/rosidl_generator_rs/resource/srv_idiomatic.rs.em b/rosidl_generator_rs/resource/srv_idiomatic.rs.em new file mode 100644 index 000000000..660f1a67c --- /dev/null +++ b/rosidl_generator_rs/resource/srv_idiomatic.rs.em @@ -0,0 +1,44 @@ +@{ +req_res_specs = [] + +for subfolder, service in srv_specs: + req_res_specs.append((subfolder, service.request_message)) + req_res_specs.append((subfolder, service.response_message)) +}@ + +@{ +TEMPLATE( + 'msg_idiomatic.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=req_res_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ + +@[for subfolder, srv_spec in srv_specs] + +@{ +type_name = srv_spec.namespaced_type.name +}@ + +#[link(name = "@(package_name)__rosidl_typesupport_c")] +extern "C" { + fn rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::ffi::c_void; +} + +// Corresponds to @(package_name)__@(subfolder)__@(type_name) +pub struct @(type_name); + +impl rosidl_runtime_rs::Service for @(type_name) { + type Request = crate::@(subfolder)::@(type_name)_Request; + type Response = crate::@(subfolder)::@(type_name)_Response; + + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } + } +} + +@[end for] diff --git a/rosidl_generator_rs/resource/srv_rmw.rs.em b/rosidl_generator_rs/resource/srv_rmw.rs.em new file mode 100644 index 000000000..6ba55f1f5 --- /dev/null +++ b/rosidl_generator_rs/resource/srv_rmw.rs.em @@ -0,0 +1,44 @@ +@{ +req_res_specs = [] + +for subfolder, service in srv_specs: + req_res_specs.append((subfolder, service.request_message)) + req_res_specs.append((subfolder, service.response_message)) +}@ + +@{ +TEMPLATE( + 'msg_rmw.rs.em', + package_name=package_name, interface_path=interface_path, + msg_specs=req_res_specs, + get_rs_name=get_rs_name, get_rmw_rs_type=get_rmw_rs_type, + pre_field_serde=pre_field_serde, + get_idiomatic_rs_type=get_idiomatic_rs_type, + constant_value_to_rs=constant_value_to_rs) +}@ + +@[for subfolder, srv_spec in srv_specs] + +@{ +type_name = srv_spec.namespaced_type.name +}@ + + #[link(name = "@(package_name)__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() -> *const std::ffi::c_void; + } + + // Corresponds to @(package_name)__@(subfolder)__@(type_name) + pub struct @(type_name); + + impl rosidl_runtime_rs::Service for @(type_name) { + type Request = crate::@(subfolder)::rmw::@(type_name)_Request; + type Response = crate::@(subfolder)::rmw::@(type_name)_Response; + + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { rosidl_typesupport_c__get_service_type_support_handle__@(package_name)__@(subfolder)__@(type_name)() } + } + } + +@[end for] diff --git a/rosidl_generator_rs/rosidl_generator_rs/__init__.py b/rosidl_generator_rs/rosidl_generator_rs/__init__.py index 502d1d34d..b7850a6d8 100644 --- a/rosidl_generator_rs/rosidl_generator_rs/__init__.py +++ b/rosidl_generator_rs/rosidl_generator_rs/__init__.py @@ -23,6 +23,11 @@ import rosidl_pycommon from rosidl_parser.definition import AbstractGenericString +from rosidl_parser.definition import AbstractNestedType +from rosidl_parser.definition import AbstractSequence +from rosidl_parser.definition import AbstractString +from rosidl_parser.definition import AbstractWString +from rosidl_parser.definition import Action from rosidl_parser.definition import Array from rosidl_parser.definition import BasicType from rosidl_parser.definition import BoundedSequence @@ -86,6 +91,10 @@ def generate_rs(generator_arguments_file, typesupport_impls): os.path.join(template_dir, 'srv.rs.em'): ['rust/src/%s.rs'], } + mapping_actions = { + os.path.join(template_dir, 'action.rs.em'): ['rust/src/%s.rs'], + } + # Ensure the required templates exist for template_file in mapping_msgs.keys(): assert os.path.exists(template_file), \ @@ -93,6 +102,9 @@ def generate_rs(generator_arguments_file, typesupport_impls): for template_file in mapping_srvs.keys(): assert os.path.exists(template_file), \ 'Services template file %s not found' % template_file + for template_file in mapping_actions.keys(): + assert os.path.exists(template_file), \ + 'Actions template file %s not found' % template_file data = { 'pre_field_serde': pre_field_serde, @@ -107,6 +119,7 @@ def generate_rs(generator_arguments_file, typesupport_impls): convert_lower_case_underscore_to_camel_case, 'msg_specs': [], 'srv_specs': [], + 'action_specs': [], 'package_name': args['package_name'], 'typesupport_impls': typesupport_impls, 'interface_path': idl_rel_path, @@ -121,6 +134,9 @@ def generate_rs(generator_arguments_file, typesupport_impls): for service in idl_content.get_elements_of_type(Service): data['srv_specs'].append(('srv', service)) + for action in idl_content.get_elements_of_type(Action): + data['action_specs'].append(('action', action)) + if data['msg_specs']: for template_file, generated_filenames in mapping_msgs.items(): for generated_filename in generated_filenames: @@ -143,6 +159,17 @@ def generate_rs(generator_arguments_file, typesupport_impls): generated_file, minimum_timestamp=latest_target_timestamp) + if data['action_specs']: + for template_file, generated_filenames in mapping_actions.items(): + for generated_filename in generated_filenames: + generated_file = os.path.join(args['output_dir'], + generated_filename % 'action') + rosidl_pycommon.expand_template( + os.path.join(template_dir, template_file), + data.copy(), + generated_file, + minimum_timestamp=latest_target_timestamp) + rosidl_pycommon.expand_template( os.path.join(template_dir, 'lib.rs.em'), data.copy(), diff --git a/rosidl_runtime_rs/src/lib.rs b/rosidl_runtime_rs/src/lib.rs index 93f844192..7c5bad461 100644 --- a/rosidl_runtime_rs/src/lib.rs +++ b/rosidl_runtime_rs/src/lib.rs @@ -9,4 +9,4 @@ mod string; pub use string::{BoundedString, BoundedWString, String, StringExceedsBoundsError, WString}; mod traits; -pub use traits::{Message, RmwMessage, SequenceAlloc, Service}; +pub use traits::{Action, ActionImpl, Message, RmwMessage, SequenceAlloc, Service}; diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index 15f206108..1e0908544 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -41,7 +41,7 @@ pub trait RmwMessage: Clone + Debug + Default + Send + Sync + Message { const TYPE_NAME: &'static str; /// Get a pointer to the correct `rosidl_message_type_support_t` structure. - fn get_type_support() -> *const std::os::raw::c_void; + fn get_type_support() -> *const std::ffi::c_void; } /// Trait for types that can be used in a `rclrs::Subscription` and a `rclrs::Publisher`. @@ -157,5 +157,97 @@ pub trait Service: 'static { type Response: Message; /// Get a pointer to the correct `rosidl_service_type_support_t` structure. - fn get_type_support() -> *const std::os::raw::c_void; + fn get_type_support() -> *const std::ffi::c_void; } + +/// Trait for actions. +/// +/// User code never needs to call this trait's method, much less implement this trait. +pub trait Action: 'static { + /// The goal message associated with this action. + type Goal: Message; + + /// The result message associated with this action. + type Result: Message; + + /// The feedback message associated with this action. + type Feedback: Message; + + /// Get a pointer to the correct `rosidl_action_type_support_t` structure. + fn get_type_support() -> *const std::ffi::c_void; +} + +/// Trait for action implementation details. +/// +/// User code never needs to implement this trait, nor use its associated types. +pub trait ActionImpl: 'static + Action { + /// The goal_status message associated with this action. + type GoalStatusMessage: Message; + + /// The feedback message associated with this action. + type FeedbackMessage: Message; + + /// The send_goal service associated with this action. + type SendGoalService: Service; + + /// The cancel_goal service associated with this action. + type CancelGoalService: Service; + + /// 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: RmwGoalData) -> RmwGoalRequest; + + /// Get the UUID of a goal request. + fn get_goal_request_uuid(request: &RmwGoalRequest) -> &[u8; 16]; + + /// Create a goal response message with the given acceptance and timestamp. + fn create_goal_response(accepted: bool, stamp: (i32, u32)) -> RmwGoalResponse; + + /// Get the `accepted` field of a goal response. + fn get_goal_response_accepted(response: &RmwGoalResponse) -> bool; + + /// Get the `stamp` field of a goal response. + fn get_goal_response_stamp(response: &RmwGoalResponse) -> (i32, u32); + + /// Create a feedback message with the given goal ID and contents. + fn create_feedback_message( + goal_id: &[u8; 16], + feedback: RmwFeedbackData, + ) -> RmwFeedbackMessage; + + /// Get the UUID of a feedback message. + fn get_feedback_message_uuid(feedback: &RmwFeedbackMessage) -> &[u8; 16]; + + /// Get the feedback of a feedback message. + fn get_feedback_message_feedback(feedback: &RmwFeedbackMessage) + -> &RmwFeedbackData; + + /// Create a result request message with the given goal ID. + fn create_result_request(goal_id: &[u8; 16]) -> RmwResultRequest; + + /// Get the UUID of a result request. + fn get_result_request_uuid(request: &RmwResultRequest) -> &[u8; 16]; + + /// Create a result response message with the given status and contents. + fn create_result_response(status: i8, result: RmwResultData) -> RmwResultResponse; + + /// Get the result of a result response. + fn get_result_response_result(response: &RmwResultResponse) -> &RmwResultData; + + /// Get the status of a result response. + fn get_result_response_status(response: &RmwResultResponse) -> i8; +} + +// Type definitions to simplify the ActionImpl trait +pub type RmwServiceRequest = <::Request as Message>::RmwMsg; +pub type RmwServiceResponse = <::Response as Message>::RmwMsg; +pub type RmwGoalRequest = RmwServiceRequest<::SendGoalService>; +pub type RmwGoalResponse = RmwServiceResponse<::SendGoalService>; +pub type RmwGoalData = <::Goal as Message>::RmwMsg; +pub type RmwFeedbackData = <::Feedback as Message>::RmwMsg; +pub type RmwFeedbackMessage = <::FeedbackMessage as Message>::RmwMsg; +pub type RmwResultRequest = RmwServiceRequest<::GetResultService>; +pub type RmwResultResponse = RmwServiceResponse<::GetResultService>; +pub type RmwResultData = <::Result as Message>::RmwMsg; From f45a66f47dc727e3ccb13037a6c57923af1446c7 Mon Sep 17 00:00:00 2001 From: Grey Date: Wed, 30 Oct 2024 19:59:04 +0800 Subject: [PATCH 03/20] Add a .repos file for jazzy (#425) Signed-off-by: Michael X. Grey Signed-off-by: Michael X. Grey --- ros2_rust_jazzy.repos | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 ros2_rust_jazzy.repos diff --git a/ros2_rust_jazzy.repos b/ros2_rust_jazzy.repos new file mode 100644 index 000000000..3f72f560d --- /dev/null +++ b/ros2_rust_jazzy.repos @@ -0,0 +1,29 @@ +repositories: + ros2/common_interfaces: + type: git + url: https://github.com/ros2/common_interfaces.git + version: jazzy + ros2/example_interfaces: + type: git + url: https://github.com/ros2/example_interfaces.git + version: jazzy + ros2/rcl_interfaces: + type: git + url: https://github.com/ros2/rcl_interfaces.git + version: jazzy + ros2/test_interface_files: + type: git + url: https://github.com/ros2/test_interface_files.git + version: jazzy + ros2/rosidl_core: + type: git + url: https://github.com/ros2/rosidl_core.git + version: jazzy + ros2/rosidl_defaults: + type: git + url: https://github.com/ros2/rosidl_defaults.git + version: jazzy + ros2/unique_identifier_msgs: + type: git + url: https://github.com/ros2/unique_identifier_msgs.git + version: jazzy From b4e975cdb712d81a4ba9f8f2595a10c159660ac6 Mon Sep 17 00:00:00 2001 From: Grey Date: Fri, 15 Nov 2024 18:42:48 +0800 Subject: [PATCH 04/20] Use latest stable Rust CI + Fix Test Errors (#420) * Use latest stable Rust CI Signed-off-by: Michael X. Grey * Fix quotation in doc Signed-off-by: Michael X. Grey * Fix serde feature for vendored messages Signed-off-by: Michael X. Grey * Fix formatting of lists in docs Signed-off-by: Michael X. Grey * Update minimum supported Rust version based on the currently used language features Signed-off-by: Michael X. Grey * Conform to clippy style guide Signed-off-by: Michael X. Grey * Add rust toolchain as a matrix dimension Signed-off-by: Michael X. Grey * Bump declaration of minimum supported rust version Signed-off-by: Michael X. Grey * Limit the scheduled runs to once a week Signed-off-by: Michael X. Grey * Define separate stable and minimal workflows because matrix does not work with reusable actions Signed-off-by: Michael X. Grey * Reduce minimum version to 1.75 to target Noble Signed-off-by: Michael X. Grey --------- Signed-off-by: Michael X. Grey Signed-off-by: Michael X. Grey --- .../workflows/{rust.yml => rust-minimal.yml} | 9 +- .github/workflows/rust-stable.yml | 130 ++++++++++++++++++ rclrs/Cargo.toml | 7 +- rclrs/src/parameter.rs | 18 +-- rclrs/src/parameter/range.rs | 2 +- rclrs/src/subscription.rs | 10 +- rclrs/src/subscription/message_info.rs | 36 ++--- rclrs/src/wait.rs | 2 +- 8 files changed, 178 insertions(+), 36 deletions(-) rename .github/workflows/{rust.yml => rust-minimal.yml} (94%) create mode 100644 .github/workflows/rust-stable.yml diff --git a/.github/workflows/rust.yml b/.github/workflows/rust-minimal.yml similarity index 94% rename from .github/workflows/rust.yml rename to .github/workflows/rust-minimal.yml index 3fd48dba0..9b1a5bb49 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust-minimal.yml @@ -1,4 +1,4 @@ -name: Rust +name: Rust Minimal on: push: @@ -6,7 +6,10 @@ on: pull_request: branches: [ main ] schedule: - - cron: '0 0 * * *' + # Run the CI at 02:22 UTC every Tuesday + # We pick an arbitrary time outside of most of the world's work hours + # to minimize the likelihood of running alongside a heavy workload. + - cron: '22 2 * * 2' env: CARGO_TERM_COLOR: always @@ -51,7 +54,7 @@ jobs: use-ros2-testing: ${{ matrix.ros_distribution == 'rolling' }} - name: Setup Rust - uses: dtolnay/rust-toolchain@1.74.0 + uses: dtolnay/rust-toolchain@1.75 with: components: clippy, rustfmt diff --git a/.github/workflows/rust-stable.yml b/.github/workflows/rust-stable.yml new file mode 100644 index 000000000..6dc395496 --- /dev/null +++ b/.github/workflows/rust-stable.yml @@ -0,0 +1,130 @@ +name: Rust Stable + +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + schedule: + # Run the CI at 02:22 UTC every Tuesday + # We pick an arbitrary time outside of most of the world's work hours + # to minimize the likelihood of running alongside a heavy workload. + - cron: '22 2 * * 2' + +env: + CARGO_TERM_COLOR: always + +jobs: + build: + strategy: + matrix: + ros_distribution: + - humble + - iron + - rolling + include: + # Humble Hawksbill (May 2022 - May 2027) + - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest + ros_distribution: humble + ros_version: 2 + # Iron Irwini (May 2023 - November 2024) + - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-ros-base-latest + ros_distribution: iron + ros_version: 2 + # Rolling Ridley (June 2020 - Present) + - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest + ros_distribution: rolling + ros_version: 2 + runs-on: ubuntu-latest + continue-on-error: ${{ matrix.ros_distribution == 'rolling' }} + container: + image: ${{ matrix.docker_image }} + steps: + - uses: actions/checkout@v4 + + - name: Search packages in this repository + id: list_packages + run: | + echo ::set-output name=package_list::$(colcon list --names-only) + + - name: Setup ROS environment + uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: ${{ matrix.ros_distribution }} + use-ros2-testing: ${{ matrix.ros_distribution == 'rolling' }} + + - name: Setup Rust + uses: dtolnay/rust-toolchain@stable + with: + components: clippy, rustfmt + + - name: Install colcon-cargo and colcon-ros-cargo + run: | + sudo pip3 install git+https://github.com/colcon/colcon-cargo.git + sudo pip3 install git+https://github.com/colcon/colcon-ros-cargo.git + + - name: Check formatting of Rust packages + run: | + for path in $(colcon list | awk '$3 == "(ament_cargo)" { print $2 }'); do + cd $path + rustup toolchain install nightly + cargo +nightly fmt -- --check + cd - + done + + - name: Install cargo-ament-build + run: | + cargo install --debug cargo-ament-build + + - name: Build and test + id: build + uses: ros-tooling/action-ros-ci@v0.3 + with: + package-name: ${{ steps.list_packages.outputs.package_list }} + target-ros2-distro: ${{ matrix.ros_distribution }} + vcs-repo-file-url: ros2_rust_${{ matrix.ros_distribution }}.repos + + - name: Run clippy on Rust packages + run: | + cd ${{ steps.build.outputs.ros-workspace-directory-name }} + . /opt/ros/${{ matrix.ros_distribution }}/setup.sh + for path in $(colcon list | awk '$3 == "(ament_cargo)" { print $2 }'); do + cd $path + echo "Running clippy in $path" + # Run clippy for all features except generate_docs (needed for docs.rs) + if [ "$(basename $path)" = "rclrs" ]; then + cargo clippy --all-targets -F default,dyn_msg -- -D warnings + else + cargo clippy --all-targets --all-features -- -D warnings + fi + cd - + done + + - name: Run cargo test on Rust packages + run: | + cd ${{ steps.build.outputs.ros-workspace-directory-name }} + . install/setup.sh + for path in $(colcon list | awk '$3 == "(ament_cargo)" && $1 != "examples_rclrs_minimal_pub_sub" && $1 != "examples_rclrs_minimal_client_service" && $1 != "rust_pubsub" { print $2 }'); do + cd $path + echo "Running cargo test in $path" + # Run cargo test for all features except generate_docs (needed for docs.rs) + if [ "$(basename $path)" = "rclrs" ]; then + cargo test -F default,dyn_msg + elif [ "$(basename $path)" = "rosidl_runtime_rs" ]; then + cargo test -F default + else + cargo test --all-features + fi + cd - + done + + - name: Rustdoc check + run: | + cd ${{ steps.build.outputs.ros-workspace-directory-name }} + . /opt/ros/${{ matrix.ros_distribution }}/setup.sh + for path in $(colcon list | awk '$3 == "(ament_cargo)" && $1 != "examples_rclrs_minimal_pub_sub" && $1 != "examples_rclrs_minimal_client_service" && $1 != "rust_pubsub" { print $2 }'); do + cd $path + echo "Running rustdoc check in $path" + cargo rustdoc -- -D warnings + cd - + done diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index f489ea3f0..fe17cc990 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -6,7 +6,7 @@ authors = ["Esteve Fernandez ", "Nikolai Morin Parameters<'a> { /// Returns: /// * `Ok(())` if setting was successful. /// * [`Err(DeclarationError::TypeMismatch)`] if the type of the requested value is different - /// from the parameter's type. + /// from the parameter's type. /// * [`Err(DeclarationError::OutOfRange)`] if the requested value is out of the parameter's - /// range. + /// range. /// * [`Err(DeclarationError::ReadOnly)`] if the parameter is read only. pub fn set( &self, diff --git a/rclrs/src/parameter/range.rs b/rclrs/src/parameter/range.rs index 96f66d6e3..6a46d2ff0 100644 --- a/rclrs/src/parameter/range.rs +++ b/rclrs/src/parameter/range.rs @@ -32,7 +32,7 @@ impl From<()> for ParameterRanges { /// Usually only one of these ranges will be applied, but all have to be stored since: /// /// * A dynamic parameter can change its type at runtime, in which case a different range could be -/// applied. +/// applied. /// * Introspection through service calls requires all the ranges to be reported to the user. #[derive(Clone, Debug, Default)] pub struct ParameterRanges { diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 36c241d19..fbd518c21 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -272,9 +272,7 @@ where } fn execute(&self) -> Result<(), RclrsError> { - // Immediately evaluated closure, to handle SubscriptionTakeFailed - // outside this match - match (|| { + let evaluate = || { match &mut *self.callback.lock().unwrap() { AnySubscriptionCallback::Regular(cb) => { let (msg, _) = self.take()?; @@ -302,7 +300,11 @@ where } } Ok(()) - })() { + }; + + // Immediately evaluated closure, to handle SubscriptionTakeFailed + // outside this match + match evaluate() { Err(RclrsError::RclError { code: RclReturnCode::SubscriptionTakeFailed, .. diff --git a/rclrs/src/subscription/message_info.rs b/rclrs/src/subscription/message_info.rs index 010bf28ec..1adecd3ec 100644 --- a/rclrs/src/subscription/message_info.rs +++ b/rclrs/src/subscription/message_info.rs @@ -4,26 +4,28 @@ use crate::rcl_bindings::*; /// An identifier for a publisher in the local context. /// -/// To quote the `rmw` documentation: +/// To quote the [`rmw` documentation][1]: /// /// > The identifier uniquely identifies the publisher for the local context, but -/// it will not necessarily be the same identifier given in other contexts or processes -/// for the same publisher. -/// Therefore the identifier will uniquely identify the publisher within your application -/// but may disagree about the identifier for that publisher when compared to another -/// application. -/// Even with this limitation, when combined with the publisher sequence number it can -/// uniquely identify a message within your local context. -/// Publisher GIDs generated by the RMW implementation could collide at some point, in which -/// case it is not possible to distinguish which publisher sent the message. -/// The details of how GIDs are generated are RMW implementation dependent. -/// +/// > it will not necessarily be the same identifier given in other contexts or processes +/// > for the same publisher. +/// > Therefore the identifier will uniquely identify the publisher within your application +/// > but may disagree about the identifier for that publisher when compared to another +/// > application. +/// > Even with this limitation, when combined with the publisher sequence number it can +/// > uniquely identify a message within your local context. +/// > Publisher GIDs generated by the RMW implementation could collide at some point, in which +/// > case it is not possible to distinguish which publisher sent the message. +/// > The details of how GIDs are generated are RMW implementation dependent. +/// > /// > It is possible the the RMW implementation needs to reuse a publisher GID, -/// due to running out of unique identifiers or some other constraint, in which case -/// the RMW implementation may document what happens in that case, but that -/// behavior is not defined here. -/// However, this should be avoided, if at all possible, by the RMW implementation, -/// and should be unlikely to happen in practice. +/// > due to running out of unique identifiers or some other constraint, in which case +/// > the RMW implementation may document what happens in that case, but that +/// > behavior is not defined here. +/// > However, this should be avoided, if at all possible, by the RMW implementation, +/// > and should be unlikely to happen in practice. +/// +/// [1]: https://docs.ros.org/en/rolling/p/rmw/generated/structrmw__message__info__s.html#_CPPv4N18rmw_message_info_s13publisher_gidE #[derive(Clone, Debug, PartialEq, Eq)] pub struct PublisherGid { /// Bytes identifying a publisher in the RMW implementation. diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 2ef99c026..243c9d857 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -329,7 +329,7 @@ impl WaitSet { /// /// - Passing a wait set with no wait-able items in it will return an error. /// - The timeout must not be so large so as to overflow an `i64` with its nanosecond - /// representation, or an error will occur. + /// representation, or an error will occur. /// /// This list is not comprehensive, since further errors may occur in the `rmw` or `rcl` layers. /// From 066dd7c6e70d515268945ab6190793ef605fd887 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Fri, 15 Nov 2024 12:07:56 -0500 Subject: [PATCH 05/20] Update vendored interface packages (#423) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Update rclrs vendor_interfaces.py script This updates the vendor_interfaces.py script to also vendor in the action_msgs and unique_identifier_msgs packages. The script is modified to always use a list of package names rather than hard-coding the package names everywhere. * Silence certain clippy lints on generated code In case a user enforces clippy linting on these generated packages, silence expected warnings. This is already the case in rclrs, but should be applied directly to the generated packages for the sake of downstream users. The `clippy::derive_partial_eq_without_eq` lint was already being disabled for the packages vendored by rclrs, but is now moved to the rosidl_generator_rs template instead. This is necessary since we always derive the PartialEq trait, but can't necessary derive Eq, and so don't. The `clippy::upper_case_acronyms` is new and was added to account for message type names being upper-case acrynyms, like unique_identifier_msgs::msg::UUID. * Update vendored message packages This updates the message packages vendored under the rclrs `vendor` module. The vendor_interfaces.py script was used for reproducibility. The existing packages – rcl_interfaces, rosgraph_msgs, and builtin_interfaces – are only modified in that they now use the std::ffi::c_void naming for c_void instead of the std::os::raw::c_void alias and disable certain clippy lints. The action_msgs and unique_identifier_msgs packages are newly added to enable adding action support to rclrs. action_msgs is needed for interaction with action goal info and statuses, and has a dependency on unique_identifier_msgs. --- rclrs/src/vendor/action_msgs/mod.rs | 7 + rclrs/src/vendor/action_msgs/msg.rs | 457 ++++++++++++++++++ rclrs/src/vendor/action_msgs/srv.rs | 375 ++++++++++++++ rclrs/src/vendor/builtin_interfaces/mod.rs | 2 + rclrs/src/vendor/builtin_interfaces/msg.rs | 8 +- rclrs/src/vendor/mod.rs | 3 +- rclrs/src/vendor/rcl_interfaces/mod.rs | 2 + rclrs/src/vendor/rcl_interfaces/msg.rs | 44 +- rclrs/src/vendor/rcl_interfaces/srv.rs | 97 ++-- rclrs/src/vendor/rosgraph_msgs/mod.rs | 2 + rclrs/src/vendor/rosgraph_msgs/msg.rs | 4 +- .../src/vendor/unique_identifier_msgs/mod.rs | 5 + .../src/vendor/unique_identifier_msgs/msg.rs | 125 +++++ rclrs/vendor_interfaces.py | 61 +-- rosidl_generator_rs/resource/lib.rs.em | 2 + 15 files changed, 1089 insertions(+), 105 deletions(-) create mode 100644 rclrs/src/vendor/action_msgs/mod.rs create mode 100644 rclrs/src/vendor/action_msgs/msg.rs create mode 100644 rclrs/src/vendor/action_msgs/srv.rs create mode 100644 rclrs/src/vendor/unique_identifier_msgs/mod.rs create mode 100644 rclrs/src/vendor/unique_identifier_msgs/msg.rs mode change 100644 => 100755 rclrs/vendor_interfaces.py diff --git a/rclrs/src/vendor/action_msgs/mod.rs b/rclrs/src/vendor/action_msgs/mod.rs new file mode 100644 index 000000000..f43deda46 --- /dev/null +++ b/rclrs/src/vendor/action_msgs/mod.rs @@ -0,0 +1,7 @@ +#![allow(non_camel_case_types)] +#![allow(clippy::derive_partial_eq_without_eq)] +#![allow(clippy::upper_case_acronyms)] + +pub mod msg; + +pub mod srv; diff --git a/rclrs/src/vendor/action_msgs/msg.rs b/rclrs/src/vendor/action_msgs/msg.rs new file mode 100644 index 000000000..32c7b7089 --- /dev/null +++ b/rclrs/src/vendor/action_msgs/msg.rs @@ -0,0 +1,457 @@ +pub mod rmw { + #[cfg(feature = "serde")] + use serde::{Deserialize, Serialize}; + + #[link(name = "action_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__action_msgs__msg__GoalInfo( + ) -> *const std::ffi::c_void; + } + + #[link(name = "action_msgs__rosidl_generator_c")] + extern "C" { + fn action_msgs__msg__GoalInfo__init(msg: *mut GoalInfo) -> bool; + fn action_msgs__msg__GoalInfo__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn action_msgs__msg__GoalInfo__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn action_msgs__msg__GoalInfo__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to action_msgs__msg__GoalInfo + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct GoalInfo { + pub goal_id: crate::vendor::unique_identifier_msgs::msg::rmw::UUID, + pub stamp: crate::vendor::builtin_interfaces::msg::rmw::Time, + } + + impl Default for GoalInfo { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !action_msgs__msg__GoalInfo__init(&mut msg as *mut _) { + panic!("Call to action_msgs__msg__GoalInfo__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for GoalInfo { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalInfo__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalInfo__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalInfo__Sequence__copy(in_seq, out_seq as *mut _) } + } + } + + impl rosidl_runtime_rs::Message for GoalInfo { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for GoalInfo + where + Self: Sized, + { + const TYPE_NAME: &'static str = "action_msgs/msg/GoalInfo"; + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__action_msgs__msg__GoalInfo() + } + } + } + + #[link(name = "action_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__action_msgs__msg__GoalStatus( + ) -> *const std::ffi::c_void; + } + + #[link(name = "action_msgs__rosidl_generator_c")] + extern "C" { + fn action_msgs__msg__GoalStatus__init(msg: *mut GoalStatus) -> bool; + fn action_msgs__msg__GoalStatus__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn action_msgs__msg__GoalStatus__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn action_msgs__msg__GoalStatus__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to action_msgs__msg__GoalStatus + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct GoalStatus { + pub goal_info: crate::vendor::action_msgs::msg::rmw::GoalInfo, + pub status: i8, + } + + impl GoalStatus { + /// Indicates status has not been properly set. + pub const STATUS_UNKNOWN: i8 = 0; + /// The goal has been accepted and is awaiting execution. + pub const STATUS_ACCEPTED: i8 = 1; + /// The goal is currently being executed by the action server. + pub const STATUS_EXECUTING: i8 = 2; + /// The client has requested that the goal be canceled and the action server has + /// accepted the cancel request. + pub const STATUS_CANCELING: i8 = 3; + /// The goal was achieved successfully by the action server. + pub const STATUS_SUCCEEDED: i8 = 4; + /// The goal was canceled after an external request from an action client. + pub const STATUS_CANCELED: i8 = 5; + /// The goal was terminated by the action server without an external request. + pub const STATUS_ABORTED: i8 = 6; + } + + impl Default for GoalStatus { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !action_msgs__msg__GoalStatus__init(&mut msg as *mut _) { + panic!("Call to action_msgs__msg__GoalStatus__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for GoalStatus { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalStatus__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalStatus__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalStatus__Sequence__copy(in_seq, out_seq as *mut _) } + } + } + + impl rosidl_runtime_rs::Message for GoalStatus { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for GoalStatus + where + Self: Sized, + { + const TYPE_NAME: &'static str = "action_msgs/msg/GoalStatus"; + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__action_msgs__msg__GoalStatus( + ) + } + } + } + + #[link(name = "action_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__action_msgs__msg__GoalStatusArray( + ) -> *const std::ffi::c_void; + } + + #[link(name = "action_msgs__rosidl_generator_c")] + extern "C" { + fn action_msgs__msg__GoalStatusArray__init(msg: *mut GoalStatusArray) -> bool; + fn action_msgs__msg__GoalStatusArray__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn action_msgs__msg__GoalStatusArray__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn action_msgs__msg__GoalStatusArray__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to action_msgs__msg__GoalStatusArray + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct GoalStatusArray { + pub status_list: + rosidl_runtime_rs::Sequence, + } + + impl Default for GoalStatusArray { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !action_msgs__msg__GoalStatusArray__init(&mut msg as *mut _) { + panic!("Call to action_msgs__msg__GoalStatusArray__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for GoalStatusArray { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalStatusArray__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalStatusArray__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalStatusArray__Sequence__copy(in_seq, out_seq as *mut _) } + } + } + + impl rosidl_runtime_rs::Message for GoalStatusArray { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for GoalStatusArray + where + Self: Sized, + { + const TYPE_NAME: &'static str = "action_msgs/msg/GoalStatusArray"; + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__action_msgs__msg__GoalStatusArray() + } + } + } +} // mod rmw + +#[cfg(feature = "serde")] +use serde::{Deserialize, Serialize}; + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct GoalInfo { + pub goal_id: crate::vendor::unique_identifier_msgs::msg::UUID, + pub stamp: crate::vendor::builtin_interfaces::msg::Time, +} + +impl Default for GoalInfo { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::action_msgs::msg::rmw::GoalInfo::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for GoalInfo { + type RmwMsg = crate::vendor::action_msgs::msg::rmw::GoalInfo; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + goal_id: crate::vendor::unique_identifier_msgs::msg::UUID::into_rmw_message( + std::borrow::Cow::Owned(msg.goal_id), + ) + .into_owned(), + stamp: crate::vendor::builtin_interfaces::msg::Time::into_rmw_message( + std::borrow::Cow::Owned(msg.stamp), + ) + .into_owned(), + }), + std::borrow::Cow::Borrowed(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + goal_id: crate::vendor::unique_identifier_msgs::msg::UUID::into_rmw_message( + std::borrow::Cow::Borrowed(&msg.goal_id), + ) + .into_owned(), + stamp: crate::vendor::builtin_interfaces::msg::Time::into_rmw_message( + std::borrow::Cow::Borrowed(&msg.stamp), + ) + .into_owned(), + }), + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { + goal_id: crate::vendor::unique_identifier_msgs::msg::UUID::from_rmw_message( + msg.goal_id, + ), + stamp: crate::vendor::builtin_interfaces::msg::Time::from_rmw_message(msg.stamp), + } + } +} + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct GoalStatus { + pub goal_info: crate::vendor::action_msgs::msg::GoalInfo, + pub status: i8, +} + +impl GoalStatus { + /// Indicates status has not been properly set. + pub const STATUS_UNKNOWN: i8 = 0; + /// The goal has been accepted and is awaiting execution. + pub const STATUS_ACCEPTED: i8 = 1; + /// The goal is currently being executed by the action server. + pub const STATUS_EXECUTING: i8 = 2; + /// The client has requested that the goal be canceled and the action server has + /// accepted the cancel request. + pub const STATUS_CANCELING: i8 = 3; + /// The goal was achieved successfully by the action server. + pub const STATUS_SUCCEEDED: i8 = 4; + /// The goal was canceled after an external request from an action client. + pub const STATUS_CANCELED: i8 = 5; + /// The goal was terminated by the action server without an external request. + pub const STATUS_ABORTED: i8 = 6; +} + +impl Default for GoalStatus { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::action_msgs::msg::rmw::GoalStatus::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for GoalStatus { + type RmwMsg = crate::vendor::action_msgs::msg::rmw::GoalStatus; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + goal_info: crate::vendor::action_msgs::msg::GoalInfo::into_rmw_message( + std::borrow::Cow::Owned(msg.goal_info), + ) + .into_owned(), + status: msg.status, + }), + std::borrow::Cow::Borrowed(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + goal_info: crate::vendor::action_msgs::msg::GoalInfo::into_rmw_message( + std::borrow::Cow::Borrowed(&msg.goal_info), + ) + .into_owned(), + status: msg.status, + }), + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { + goal_info: crate::vendor::action_msgs::msg::GoalInfo::from_rmw_message(msg.goal_info), + status: msg.status, + } + } +} + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct GoalStatusArray { + pub status_list: Vec, +} + +impl Default for GoalStatusArray { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::action_msgs::msg::rmw::GoalStatusArray::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for GoalStatusArray { + type RmwMsg = crate::vendor::action_msgs::msg::rmw::GoalStatusArray; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + status_list: msg + .status_list + .into_iter() + .map(|elem| { + crate::vendor::action_msgs::msg::GoalStatus::into_rmw_message( + std::borrow::Cow::Owned(elem), + ) + .into_owned() + }) + .collect(), + }), + std::borrow::Cow::Borrowed(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + status_list: msg + .status_list + .iter() + .map(|elem| { + crate::vendor::action_msgs::msg::GoalStatus::into_rmw_message( + std::borrow::Cow::Borrowed(elem), + ) + .into_owned() + }) + .collect(), + }), + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { + status_list: msg + .status_list + .into_iter() + .map(crate::vendor::action_msgs::msg::GoalStatus::from_rmw_message) + .collect(), + } + } +} diff --git a/rclrs/src/vendor/action_msgs/srv.rs b/rclrs/src/vendor/action_msgs/srv.rs new file mode 100644 index 000000000..f4cf6fe53 --- /dev/null +++ b/rclrs/src/vendor/action_msgs/srv.rs @@ -0,0 +1,375 @@ +#[cfg(feature = "serde")] +use serde::{Deserialize, Serialize}; + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct CancelGoal_Request { + pub goal_info: crate::vendor::action_msgs::msg::GoalInfo, +} + +impl Default for CancelGoal_Request { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::action_msgs::srv::rmw::CancelGoal_Request::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for CancelGoal_Request { + type RmwMsg = crate::vendor::action_msgs::srv::rmw::CancelGoal_Request; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + goal_info: crate::vendor::action_msgs::msg::GoalInfo::into_rmw_message( + std::borrow::Cow::Owned(msg.goal_info), + ) + .into_owned(), + }), + std::borrow::Cow::Borrowed(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + goal_info: crate::vendor::action_msgs::msg::GoalInfo::into_rmw_message( + std::borrow::Cow::Borrowed(&msg.goal_info), + ) + .into_owned(), + }), + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { + goal_info: crate::vendor::action_msgs::msg::GoalInfo::from_rmw_message(msg.goal_info), + } + } +} + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct CancelGoal_Response { + pub return_code: i8, + pub goals_canceling: Vec, +} + +impl CancelGoal_Response { + /// Indicates the request was accepted without any errors. + /// + /// One or more goals have transitioned to the CANCELING state. The + /// goals_canceling list is not empty. + pub const ERROR_NONE: i8 = 0; + /// Indicates the request was rejected. + /// + /// No goals have transitioned to the CANCELING state. The goals_canceling list is + /// empty. + pub const ERROR_REJECTED: i8 = 1; + /// Indicates the requested goal ID does not exist. + /// + /// No goals have transitioned to the CANCELING state. The goals_canceling list is + /// empty. + pub const ERROR_UNKNOWN_GOAL_ID: i8 = 2; + /// Indicates the goal is not cancelable because it is already in a terminal state. + /// + /// No goals have transitioned to the CANCELING state. The goals_canceling list is + /// empty. + pub const ERROR_GOAL_TERMINATED: i8 = 3; +} + +impl Default for CancelGoal_Response { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::action_msgs::srv::rmw::CancelGoal_Response::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for CancelGoal_Response { + type RmwMsg = crate::vendor::action_msgs::srv::rmw::CancelGoal_Response; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + return_code: msg.return_code, + goals_canceling: msg + .goals_canceling + .into_iter() + .map(|elem| { + crate::vendor::action_msgs::msg::GoalInfo::into_rmw_message( + std::borrow::Cow::Owned(elem), + ) + .into_owned() + }) + .collect(), + }), + std::borrow::Cow::Borrowed(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + return_code: msg.return_code, + goals_canceling: msg + .goals_canceling + .iter() + .map(|elem| { + crate::vendor::action_msgs::msg::GoalInfo::into_rmw_message( + std::borrow::Cow::Borrowed(elem), + ) + .into_owned() + }) + .collect(), + }), + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { + return_code: msg.return_code, + goals_canceling: msg + .goals_canceling + .into_iter() + .map(crate::vendor::action_msgs::msg::GoalInfo::from_rmw_message) + .collect(), + } + } +} + +#[link(name = "action_msgs__rosidl_typesupport_c")] +extern "C" { + fn rosidl_typesupport_c__get_service_type_support_handle__action_msgs__srv__CancelGoal( + ) -> *const std::ffi::c_void; +} + +// Corresponds to action_msgs__srv__CancelGoal +pub struct CancelGoal; + +impl rosidl_runtime_rs::Service for CancelGoal { + type Request = crate::vendor::action_msgs::srv::CancelGoal_Request; + type Response = crate::vendor::action_msgs::srv::CancelGoal_Response; + + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_service_type_support_handle__action_msgs__srv__CancelGoal() + } + } +} + +pub mod rmw { + + #[cfg(feature = "serde")] + use serde::{Deserialize, Serialize}; + + #[link(name = "action_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__action_msgs__srv__CancelGoal_Request( + ) -> *const std::ffi::c_void; + } + + #[link(name = "action_msgs__rosidl_generator_c")] + extern "C" { + fn action_msgs__srv__CancelGoal_Request__init(msg: *mut CancelGoal_Request) -> bool; + fn action_msgs__srv__CancelGoal_Request__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn action_msgs__srv__CancelGoal_Request__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn action_msgs__srv__CancelGoal_Request__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to action_msgs__srv__CancelGoal_Request + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct CancelGoal_Request { + pub goal_info: crate::vendor::action_msgs::msg::rmw::GoalInfo, + } + + impl Default for CancelGoal_Request { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !action_msgs__srv__CancelGoal_Request__init(&mut msg as *mut _) { + panic!("Call to action_msgs__srv__CancelGoal_Request__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for CancelGoal_Request { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__srv__CancelGoal_Request__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__srv__CancelGoal_Request__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { + action_msgs__srv__CancelGoal_Request__Sequence__copy(in_seq, out_seq as *mut _) + } + } + } + + impl rosidl_runtime_rs::Message for CancelGoal_Request { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for CancelGoal_Request + where + Self: Sized, + { + const TYPE_NAME: &'static str = "action_msgs/srv/CancelGoal_Request"; + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__action_msgs__srv__CancelGoal_Request() + } + } + } + + #[link(name = "action_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__action_msgs__srv__CancelGoal_Response( + ) -> *const std::ffi::c_void; + } + + #[link(name = "action_msgs__rosidl_generator_c")] + extern "C" { + fn action_msgs__srv__CancelGoal_Response__init(msg: *mut CancelGoal_Response) -> bool; + fn action_msgs__srv__CancelGoal_Response__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn action_msgs__srv__CancelGoal_Response__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn action_msgs__srv__CancelGoal_Response__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to action_msgs__srv__CancelGoal_Response + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct CancelGoal_Response { + pub return_code: i8, + pub goals_canceling: + rosidl_runtime_rs::Sequence, + } + + impl CancelGoal_Response { + /// Indicates the request was accepted without any errors. + /// + /// One or more goals have transitioned to the CANCELING state. The + /// goals_canceling list is not empty. + pub const ERROR_NONE: i8 = 0; + /// Indicates the request was rejected. + /// + /// No goals have transitioned to the CANCELING state. The goals_canceling list is + /// empty. + pub const ERROR_REJECTED: i8 = 1; + /// Indicates the requested goal ID does not exist. + /// + /// No goals have transitioned to the CANCELING state. The goals_canceling list is + /// empty. + pub const ERROR_UNKNOWN_GOAL_ID: i8 = 2; + /// Indicates the goal is not cancelable because it is already in a terminal state. + /// + /// No goals have transitioned to the CANCELING state. The goals_canceling list is + /// empty. + pub const ERROR_GOAL_TERMINATED: i8 = 3; + } + + impl Default for CancelGoal_Response { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !action_msgs__srv__CancelGoal_Response__init(&mut msg as *mut _) { + panic!("Call to action_msgs__srv__CancelGoal_Response__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for CancelGoal_Response { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__srv__CancelGoal_Response__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__srv__CancelGoal_Response__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { + action_msgs__srv__CancelGoal_Response__Sequence__copy(in_seq, out_seq as *mut _) + } + } + } + + impl rosidl_runtime_rs::Message for CancelGoal_Response { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for CancelGoal_Response + where + Self: Sized, + { + const TYPE_NAME: &'static str = "action_msgs/srv/CancelGoal_Response"; + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__action_msgs__srv__CancelGoal_Response() + } + } + } + + #[link(name = "action_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_service_type_support_handle__action_msgs__srv__CancelGoal( + ) -> *const std::ffi::c_void; + } + + // Corresponds to action_msgs__srv__CancelGoal + pub struct CancelGoal; + + impl rosidl_runtime_rs::Service for CancelGoal { + type Request = crate::vendor::action_msgs::srv::rmw::CancelGoal_Request; + type Response = crate::vendor::action_msgs::srv::rmw::CancelGoal_Response; + + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_service_type_support_handle__action_msgs__srv__CancelGoal( + ) + } + } + } +} // mod rmw diff --git a/rclrs/src/vendor/builtin_interfaces/mod.rs b/rclrs/src/vendor/builtin_interfaces/mod.rs index a6365b3b8..4c61d56a1 100644 --- a/rclrs/src/vendor/builtin_interfaces/mod.rs +++ b/rclrs/src/vendor/builtin_interfaces/mod.rs @@ -1,3 +1,5 @@ #![allow(non_camel_case_types)] +#![allow(clippy::derive_partial_eq_without_eq)] +#![allow(clippy::upper_case_acronyms)] pub mod msg; diff --git a/rclrs/src/vendor/builtin_interfaces/msg.rs b/rclrs/src/vendor/builtin_interfaces/msg.rs index 371803171..fd2ec47cd 100644 --- a/rclrs/src/vendor/builtin_interfaces/msg.rs +++ b/rclrs/src/vendor/builtin_interfaces/msg.rs @@ -5,7 +5,7 @@ pub mod rmw { #[link(name = "builtin_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__builtin_interfaces__msg__Duration( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "builtin_interfaces__rosidl_generator_c")] @@ -80,7 +80,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "builtin_interfaces/msg/Duration"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__builtin_interfaces__msg__Duration() @@ -91,7 +91,7 @@ pub mod rmw { #[link(name = "builtin_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__builtin_interfaces__msg__Time( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "builtin_interfaces__rosidl_generator_c")] @@ -166,7 +166,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "builtin_interfaces/msg/Time"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__builtin_interfaces__msg__Time( diff --git a/rclrs/src/vendor/mod.rs b/rclrs/src/vendor/mod.rs index fe87087b1..df638ee7f 100644 --- a/rclrs/src/vendor/mod.rs +++ b/rclrs/src/vendor/mod.rs @@ -1,7 +1,8 @@ //! Created by vendor_interfaces.py #![allow(dead_code)] -#![allow(clippy::derive_partial_eq_without_eq)] +pub mod action_msgs; pub mod builtin_interfaces; pub mod rcl_interfaces; pub mod rosgraph_msgs; +pub mod unique_identifier_msgs; diff --git a/rclrs/src/vendor/rcl_interfaces/mod.rs b/rclrs/src/vendor/rcl_interfaces/mod.rs index ff96b59a9..f43deda46 100644 --- a/rclrs/src/vendor/rcl_interfaces/mod.rs +++ b/rclrs/src/vendor/rcl_interfaces/mod.rs @@ -1,4 +1,6 @@ #![allow(non_camel_case_types)] +#![allow(clippy::derive_partial_eq_without_eq)] +#![allow(clippy::upper_case_acronyms)] pub mod msg; diff --git a/rclrs/src/vendor/rcl_interfaces/msg.rs b/rclrs/src/vendor/rcl_interfaces/msg.rs index 39c029ae1..7f6dbcc58 100644 --- a/rclrs/src/vendor/rcl_interfaces/msg.rs +++ b/rclrs/src/vendor/rcl_interfaces/msg.rs @@ -5,7 +5,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__FloatingPointRange( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -83,7 +83,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/FloatingPointRange"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__FloatingPointRange() @@ -94,7 +94,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__IntegerRange( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -170,7 +170,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/IntegerRange"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__IntegerRange() @@ -181,7 +181,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ListParametersResult( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -260,7 +260,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/ListParametersResult"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ListParametersResult() @@ -271,7 +271,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__Log( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -364,7 +364,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/Log"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__Log() @@ -375,7 +375,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterDescriptor( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -464,7 +464,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/ParameterDescriptor"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterDescriptor() @@ -475,7 +475,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterEventDescriptors( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -566,7 +566,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/ParameterEventDescriptors"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterEventDescriptors() @@ -577,7 +577,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterEvent( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -660,7 +660,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/ParameterEvent"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterEvent() @@ -671,7 +671,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__Parameter( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -746,7 +746,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/Parameter"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__Parameter() @@ -757,7 +757,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterType( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -845,7 +845,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/ParameterType"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterType() @@ -856,7 +856,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterValue( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -941,7 +941,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/ParameterValue"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterValue() @@ -952,7 +952,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__SetParametersResult( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1029,7 +1029,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/SetParametersResult"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__SetParametersResult() diff --git a/rclrs/src/vendor/rcl_interfaces/srv.rs b/rclrs/src/vendor/rcl_interfaces/srv.rs index 221d7eac1..3f43398a1 100644 --- a/rclrs/src/vendor/rcl_interfaces/srv.rs +++ b/rclrs/src/vendor/rcl_interfaces/srv.rs @@ -582,7 +582,7 @@ impl rosidl_runtime_rs::Message for SetParameters_Response { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__DescribeParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__DescribeParameters @@ -592,7 +592,7 @@ impl rosidl_runtime_rs::Service for DescribeParameters { type Request = crate::vendor::rcl_interfaces::srv::DescribeParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::DescribeParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__DescribeParameters() @@ -603,7 +603,7 @@ impl rosidl_runtime_rs::Service for DescribeParameters { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__GetParameters @@ -613,7 +613,7 @@ impl rosidl_runtime_rs::Service for GetParameters { type Request = crate::vendor::rcl_interfaces::srv::GetParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::GetParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameters() @@ -624,7 +624,7 @@ impl rosidl_runtime_rs::Service for GetParameters { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameterTypes( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__GetParameterTypes @@ -634,7 +634,7 @@ impl rosidl_runtime_rs::Service for GetParameterTypes { type Request = crate::vendor::rcl_interfaces::srv::GetParameterTypes_Request; type Response = crate::vendor::rcl_interfaces::srv::GetParameterTypes_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameterTypes() @@ -645,7 +645,7 @@ impl rosidl_runtime_rs::Service for GetParameterTypes { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__ListParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__ListParameters @@ -655,7 +655,7 @@ impl rosidl_runtime_rs::Service for ListParameters { type Request = crate::vendor::rcl_interfaces::srv::ListParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::ListParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__ListParameters() @@ -666,7 +666,7 @@ impl rosidl_runtime_rs::Service for ListParameters { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParametersAtomically( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__SetParametersAtomically @@ -676,7 +676,7 @@ impl rosidl_runtime_rs::Service for SetParametersAtomically { type Request = crate::vendor::rcl_interfaces::srv::SetParametersAtomically_Request; type Response = crate::vendor::rcl_interfaces::srv::SetParametersAtomically_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParametersAtomically() @@ -687,7 +687,7 @@ impl rosidl_runtime_rs::Service for SetParametersAtomically { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__SetParameters @@ -697,7 +697,7 @@ impl rosidl_runtime_rs::Service for SetParameters { type Request = crate::vendor::rcl_interfaces::srv::SetParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::SetParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParameters() @@ -706,13 +706,14 @@ impl rosidl_runtime_rs::Service for SetParameters { } pub mod rmw { + #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__DescribeParameters_Request( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -799,7 +800,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/DescribeParameters_Request"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__DescribeParameters_Request() @@ -810,7 +811,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__DescribeParameters_Response( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -902,7 +903,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/DescribeParameters_Response"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__DescribeParameters_Response() @@ -913,7 +914,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameters_Request( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -996,7 +997,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/GetParameters_Request"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameters_Request() @@ -1007,7 +1008,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameters_Response( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1091,7 +1092,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/GetParameters_Response"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameters_Response() @@ -1102,7 +1103,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameterTypes_Request( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1185,7 +1186,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/GetParameterTypes_Request"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameterTypes_Request() @@ -1196,7 +1197,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameterTypes_Response( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1283,7 +1284,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/GetParameterTypes_Response"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameterTypes_Response() @@ -1294,7 +1295,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__ListParameters_Request( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1382,7 +1383,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/ListParameters_Request"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__ListParameters_Request() @@ -1393,7 +1394,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__ListParameters_Response( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1476,7 +1477,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/ListParameters_Response"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__ListParameters_Response() @@ -1487,7 +1488,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParametersAtomically_Request( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1576,7 +1577,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/SetParametersAtomically_Request"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParametersAtomically_Request() @@ -1587,7 +1588,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParametersAtomically_Response( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1676,7 +1677,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/SetParametersAtomically_Response"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParametersAtomically_Response() @@ -1687,7 +1688,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParameters_Request( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1771,7 +1772,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/SetParameters_Request"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParameters_Request() @@ -1782,7 +1783,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParameters_Response( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1867,7 +1868,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/SetParameters_Response"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParameters_Response() @@ -1878,7 +1879,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__DescribeParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__DescribeParameters @@ -1888,7 +1889,7 @@ pub mod rmw { type Request = crate::vendor::rcl_interfaces::srv::rmw::DescribeParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::rmw::DescribeParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__DescribeParameters() @@ -1899,7 +1900,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__GetParameters @@ -1909,7 +1910,7 @@ pub mod rmw { type Request = crate::vendor::rcl_interfaces::srv::rmw::GetParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::rmw::GetParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameters() @@ -1920,7 +1921,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameterTypes( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__GetParameterTypes @@ -1930,7 +1931,7 @@ pub mod rmw { type Request = crate::vendor::rcl_interfaces::srv::rmw::GetParameterTypes_Request; type Response = crate::vendor::rcl_interfaces::srv::rmw::GetParameterTypes_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameterTypes() @@ -1941,7 +1942,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__ListParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__ListParameters @@ -1951,7 +1952,7 @@ pub mod rmw { type Request = crate::vendor::rcl_interfaces::srv::rmw::ListParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::rmw::ListParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__ListParameters() @@ -1962,7 +1963,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParametersAtomically( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__SetParametersAtomically @@ -1972,7 +1973,7 @@ pub mod rmw { type Request = crate::vendor::rcl_interfaces::srv::rmw::SetParametersAtomically_Request; type Response = crate::vendor::rcl_interfaces::srv::rmw::SetParametersAtomically_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParametersAtomically() @@ -1983,7 +1984,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__SetParameters @@ -1993,7 +1994,7 @@ pub mod rmw { type Request = crate::vendor::rcl_interfaces::srv::rmw::SetParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::rmw::SetParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParameters() diff --git a/rclrs/src/vendor/rosgraph_msgs/mod.rs b/rclrs/src/vendor/rosgraph_msgs/mod.rs index a6365b3b8..4c61d56a1 100644 --- a/rclrs/src/vendor/rosgraph_msgs/mod.rs +++ b/rclrs/src/vendor/rosgraph_msgs/mod.rs @@ -1,3 +1,5 @@ #![allow(non_camel_case_types)] +#![allow(clippy::derive_partial_eq_without_eq)] +#![allow(clippy::upper_case_acronyms)] pub mod msg; diff --git a/rclrs/src/vendor/rosgraph_msgs/msg.rs b/rclrs/src/vendor/rosgraph_msgs/msg.rs index 1f4af8b70..6190bfe62 100644 --- a/rclrs/src/vendor/rosgraph_msgs/msg.rs +++ b/rclrs/src/vendor/rosgraph_msgs/msg.rs @@ -5,7 +5,7 @@ pub mod rmw { #[link(name = "rosgraph_msgs__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rosgraph_msgs__msg__Clock( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rosgraph_msgs__rosidl_generator_c")] @@ -77,7 +77,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rosgraph_msgs/msg/Clock"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rosgraph_msgs__msg__Clock() diff --git a/rclrs/src/vendor/unique_identifier_msgs/mod.rs b/rclrs/src/vendor/unique_identifier_msgs/mod.rs new file mode 100644 index 000000000..4c61d56a1 --- /dev/null +++ b/rclrs/src/vendor/unique_identifier_msgs/mod.rs @@ -0,0 +1,5 @@ +#![allow(non_camel_case_types)] +#![allow(clippy::derive_partial_eq_without_eq)] +#![allow(clippy::upper_case_acronyms)] + +pub mod msg; diff --git a/rclrs/src/vendor/unique_identifier_msgs/msg.rs b/rclrs/src/vendor/unique_identifier_msgs/msg.rs new file mode 100644 index 000000000..7d6085fb1 --- /dev/null +++ b/rclrs/src/vendor/unique_identifier_msgs/msg.rs @@ -0,0 +1,125 @@ +pub mod rmw { + #[cfg(feature = "serde")] + use serde::{Deserialize, Serialize}; + + #[link(name = "unique_identifier_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__unique_identifier_msgs__msg__UUID( + ) -> *const std::ffi::c_void; + } + + #[link(name = "unique_identifier_msgs__rosidl_generator_c")] + extern "C" { + fn unique_identifier_msgs__msg__UUID__init(msg: *mut UUID) -> bool; + fn unique_identifier_msgs__msg__UUID__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn unique_identifier_msgs__msg__UUID__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn unique_identifier_msgs__msg__UUID__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to unique_identifier_msgs__msg__UUID + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct UUID { + pub uuid: [u8; 16], + } + + impl Default for UUID { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !unique_identifier_msgs__msg__UUID__init(&mut msg as *mut _) { + panic!("Call to unique_identifier_msgs__msg__UUID__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for UUID { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { unique_identifier_msgs__msg__UUID__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { unique_identifier_msgs__msg__UUID__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { unique_identifier_msgs__msg__UUID__Sequence__copy(in_seq, out_seq as *mut _) } + } + } + + impl rosidl_runtime_rs::Message for UUID { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for UUID + where + Self: Sized, + { + const TYPE_NAME: &'static str = "unique_identifier_msgs/msg/UUID"; + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__unique_identifier_msgs__msg__UUID() + } + } + } +} // mod rmw + +#[cfg(feature = "serde")] +use serde::{Deserialize, Serialize}; + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct UUID { + pub uuid: [u8; 16], +} + +impl Default for UUID { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::unique_identifier_msgs::msg::rmw::UUID::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for UUID { + type RmwMsg = crate::vendor::unique_identifier_msgs::msg::rmw::UUID; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => { + std::borrow::Cow::Owned(Self::RmwMsg { uuid: msg.uuid }) + } + std::borrow::Cow::Borrowed(msg) => { + std::borrow::Cow::Owned(Self::RmwMsg { uuid: msg.uuid }) + } + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { uuid: msg.uuid } + } +} diff --git a/rclrs/vendor_interfaces.py b/rclrs/vendor_interfaces.py old mode 100644 new mode 100755 index 6a0066869..8ffb4e4e0 --- a/rclrs/vendor_interfaces.py +++ b/rclrs/vendor_interfaces.py @@ -1,65 +1,70 @@ -# This script produces the `vendor` module inside `rclrs` by copying -# the generated code for the `rosgraph_msgs` and `rcl_interfaces` packages and -# its dependency `builtin_interfaces` and adjusting the submodule paths in the -# code. +#!/usr/bin/env python3 +# This script produces the `vendor` module inside `rclrs` by copying the +# generated code for the `rosgraph_msgs`, `rcl_interfaces`, and `action_msgs` +# packages and their dependencies `builtin_interfaces` and +# `unique_identifier_msgs` and adjusting the submodule paths in the code. # If these packages, or the `rosidl_generator_rs`, get changed, you can # update the `vendor` module by running this script. -# The purpose is to avoid an external dependency on `rcl_interfaces`, which -# is not published on crates.io. +# The purpose is to avoid an external dependency on these message packages, +# which are not published on crates.io. import argparse from pathlib import Path import shutil import subprocess +vendored_packages = [ + "action_msgs", + "builtin_interfaces", + "rcl_interfaces", + "rosgraph_msgs", + "unique_identifier_msgs", +] + def get_args(): - parser = argparse.ArgumentParser(description='Vendor the rcl_interfaces, builtin_interfaces and rosgraph_msgs packages into rclrs') + parser = argparse.ArgumentParser(description='Vendor interface packages into rclrs') parser.add_argument('install_base', metavar='install_base', type=Path, help='the install base (must have non-merged layout)') return parser.parse_args() -def adjust(pkg, text): - text = text.replace('builtin_interfaces::', 'crate::vendor::builtin_interfaces::') - text = text.replace('rcl_interfaces::', 'crate::vendor::rcl_interfaces::') - text = text.replace('rosgraph_msgs::', 'crate::vendor::rosgraph_msgs::') - text = text.replace('crate::msg', f'crate::vendor::{pkg}::msg') - text = text.replace('crate::srv', f'crate::vendor::{pkg}::srv') +def adjust(current_package, text): + for pkg in vendored_packages: + text = text.replace(f'{pkg}::', f'crate::vendor::{pkg}::') + text = text.replace('crate::msg', f'crate::vendor::{current_package}::msg') + text = text.replace('crate::srv', f'crate::vendor::{current_package}::srv') + text = text.replace('crate::action', f'crate::vendor::{current_package}::action') return text def copy_adjusted(pkg, src, dst): dst.write_text(adjust(pkg, src.read_text())) subprocess.check_call(['rustfmt', str(dst)]) -mod_contents = """//! Created by {} -#![allow(dead_code)] -#![allow(clippy::derive_partial_eq_without_eq)] - -pub mod builtin_interfaces; -pub mod rcl_interfaces; -pub mod rosgraph_msgs; -""".format(Path(__file__).name) - def main(): args = get_args() assert args.install_base.is_dir(), "Install base does not exist" - assert (args.install_base / 'builtin_interfaces').is_dir(), "Install base does not contain builtin_interfaces" - assert (args.install_base / 'rcl_interfaces').is_dir(), "Install base does not contain rcl_interfaces" - assert (args.install_base / 'rosgraph_msgs').is_dir(), "Install base does not contain rosgraph_msgs" + for pkg in vendored_packages: + assert (args.install_base / pkg).is_dir(), f"Install base does not contain {pkg}" rclrs_root = Path(__file__).parent vendor_dir = rclrs_root / 'src' / 'vendor' if vendor_dir.exists(): shutil.rmtree(vendor_dir) - for pkg in ['builtin_interfaces', 'rcl_interfaces', 'rosgraph_msgs']: + for pkg in vendored_packages: src = args.install_base / pkg / 'share' / pkg / 'rust' / 'src' dst = vendor_dir / pkg dst.mkdir(parents=True) copy_adjusted(pkg, src / 'msg.rs', dst / 'msg.rs') if (src / 'srv.rs').is_file(): copy_adjusted(pkg, src / 'srv.rs', dst / 'srv.rs') + if (src / 'action.rs').is_file(): + copy_adjusted(pkg, src / 'action.rs', dst / 'action.rs') copy_adjusted(pkg, src / 'lib.rs', dst / 'mod.rs') # Rename lib.rs to mod.rs - (vendor_dir / 'mod.rs').write_text(mod_contents) - + mod_contents = "//! Created by {}\n".format(Path(__file__).name) + mod_contents += "#![allow(dead_code)]\n" + mod_contents += "\n" + for pkg in vendored_packages: + mod_contents += f"pub mod {pkg};\n" + (vendor_dir / 'mod.rs').write_text(mod_contents) if __name__ == '__main__': main() diff --git a/rosidl_generator_rs/resource/lib.rs.em b/rosidl_generator_rs/resource/lib.rs.em index 79a0e1def..1ef77924c 100644 --- a/rosidl_generator_rs/resource/lib.rs.em +++ b/rosidl_generator_rs/resource/lib.rs.em @@ -1,4 +1,6 @@ #![allow(non_camel_case_types)] +#![allow(clippy::derive_partial_eq_without_eq)] +#![allow(clippy::upper_case_acronyms)] @[if len(msg_specs) > 0]@ pub mod msg; From 29fe10ef7f9bd33fcd0fdb795f562e5a19e0aa0d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 16 Nov 2024 16:05:28 +0000 Subject: [PATCH 06/20] Migrate Context, Executor, and Node creation to new API Signed-off-by: Michael X. Grey --- rclrs/src/context.rs | 66 ++++--- rclrs/src/error.rs | 21 +++ rclrs/src/executor.rs | 123 ++++++++----- rclrs/src/lib.rs | 65 ------- rclrs/src/node.rs | 162 ++++++++++-------- rclrs/src/node/graph.rs | 17 +- .../src/node/{builder.rs => node_options.rs} | 153 +++++++++-------- rclrs/src/parameter.rs | 114 +++++++----- rclrs/src/parameter/service.rs | 106 +++++++----- rclrs/src/parameter/value.rs | 15 +- rclrs/src/test_helpers/graph_helpers.rs | 19 +- rclrs/src/time_source.rs | 35 ++-- rclrs/src/wait.rs | 2 +- rclrs/src/wait/guard_condition.rs | 6 +- 14 files changed, 509 insertions(+), 395 deletions(-) rename rclrs/src/node/{builder.rs => node_options.rs} (75%) diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 90c8fbd3c..b218fc24b 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -6,7 +6,7 @@ use std::{ vec::Vec, }; -use crate::{rcl_bindings::*, RclrsError, ToResult}; +use crate::{rcl_bindings::*, RclrsError, ToResult, Executor}; /// This is locked whenever initializing or dropping any middleware entity /// because we have found issues in RCL and some RMW implementations that @@ -70,34 +70,41 @@ pub(crate) struct ContextHandle { pub(crate) rcl_context: Mutex, } +impl Default for Context { + fn default() -> Self { + // SAFETY: It should always be valid to instantiate a context with no + // arguments, no parameters, no options, etc. + Self::new([], InitOptions::default()) + .expect("Failed to instantiate a default context") + } +} + impl Context { /// Creates a new context. /// - /// Usually this would be called with `std::env::args()`, analogously to `rclcpp::init()`. - /// See also the official "Passing ROS arguments to nodes via the command-line" tutorial. + /// * `args` - A sequence of strings that resembles command line arguments + /// that users can pass into a ROS executable. See [the official tutorial][1] + /// to know what these arguments may look like. To simply pass in the arguments + /// that the user has provided from the command line, call [`Self::from_env`] + /// or [`Self::default_from_env`] instead. /// - /// Creating a context will fail if the args contain invalid ROS arguments. + /// * `options` - Additional options that your application can use to override + /// settings that would otherwise be determined by the environment. /// - /// # Example - /// ``` - /// # use rclrs::Context; - /// assert!(Context::new([]).is_ok()); - /// let invalid_remapping = ["--ros-args", "-r", ":=:*/]"].map(String::from); - /// assert!(Context::new(invalid_remapping).is_err()); - /// ``` - pub fn new(args: impl IntoIterator) -> Result { - Self::new_with_options(args, InitOptions::new()) - } - - /// Same as [`Context::new`] except you can additionally provide initialization options. + /// Creating a context will fail if `args` contains invalid ROS arguments. /// /// # Example /// ``` /// use rclrs::{Context, InitOptions}; - /// let context = Context::new_with_options([], InitOptions::new().with_domain_id(Some(5))).unwrap(); + /// let context = Context::new( + /// std::env::args(), + /// InitOptions::new().with_domain_id(Some(5)), + /// ).unwrap(); /// assert_eq!(context.domain_id(), 5); - /// ```` - pub fn new_with_options( + /// ``` + /// + /// [1]: https://docs.ros.org/en/rolling/How-To-Guides/Node-arguments.html + pub fn new( args: impl IntoIterator, options: InitOptions, ) -> Result { @@ -150,6 +157,23 @@ impl Context { }) } + /// Same as [`Self::new`] but [`std::env::args`] is automatically passed in + /// for `args`. + pub fn from_env(options: InitOptions) -> Result { + Self::new(std::env::args(), options) + } + + /// Same as [`Self::from_env`] but the default [`InitOptions`] is passed in + /// for `options`. + pub fn default_from_env() -> Result { + Self::new(std::env::args(), InitOptions::default()) + } + + /// Create a basic executor that comes built into rclrs. + pub fn create_basic_executor(&self) -> Executor { + Executor::new(Arc::clone(&self.handle)) + } + /// Returns the ROS domain ID that the context is using. /// /// The domain ID controls which nodes can send messages to each other, see the [ROS 2 concept article][1]. @@ -250,14 +274,14 @@ mod tests { #[test] fn test_create_context() -> Result<(), RclrsError> { // If the context fails to be created, this will cause a panic - let _ = Context::new(vec![])?; + let _ = Context::new(vec![], InitOptions::default())?; Ok(()) } #[test] fn test_context_ok() -> Result<(), RclrsError> { // If the context fails to be created, this will cause a panic - let created_context = Context::new(vec![]).unwrap(); + let created_context = Context::new(vec![], InitOptions::default()).unwrap(); assert!(created_context.ok()); Ok(()) diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index 3eba2549f..515a5b95c 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -352,3 +352,24 @@ impl ToResult for rcl_ret_t { to_rclrs_result(*self) } } + +/// A helper trait to disregard timeouts as not an error. +pub trait RclrsErrorFilter { + /// If the result was a timeout error, change it to `Ok(())`. + fn timeout_ok(self) -> Result<(), RclrsError>; +} + +impl RclrsErrorFilter for Result<(), RclrsError> { + fn timeout_ok(self) -> Result<(), RclrsError> { + match self { + Ok(()) => Ok(()), + Err(err) => { + if matches!(err, RclrsError::RclError { code: RclReturnCode::Timeout, .. }) { + return Ok(()); + } + + Err(err) + } + } + } +} diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 37c43a68e..63951afef 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,48 +1,61 @@ -use crate::{rcl_bindings::rcl_context_is_valid, Node, RclReturnCode, RclrsError, WaitSet}; +use crate::{ + rcl_bindings::rcl_context_is_valid, + Node, RclrsError, WaitSet, ContextHandle, NodeOptions, WeakNode, +}; use std::{ - sync::{Arc, Mutex, Weak}, + sync::{Arc, Mutex}, time::Duration, }; /// Single-threaded executor implementation. -pub struct SingleThreadedExecutor { - nodes_mtx: Mutex>>, +pub struct Executor { + context: Arc, + nodes_mtx: Mutex>, } -impl Default for SingleThreadedExecutor { - fn default() -> Self { - Self::new() +impl Executor { + /// Create a [`Node`] that will run on this Executor. + pub fn create_node( + &self, + options: impl Into, + ) -> Result { + let options: NodeOptions = options.into(); + let node = options.build(&self.context)?; + self.nodes_mtx.lock().unwrap().push(node.downgrade()); + Ok(node) } -} -impl SingleThreadedExecutor { - /// Creates a new executor. - pub fn new() -> Self { - SingleThreadedExecutor { - nodes_mtx: Mutex::new(Vec::new()), - } - } + /// Spin the Executor. The current thread will be blocked until the Executor + /// stops spinning. + /// + /// [`SpinOptions`] can be used to automatically stop the spinning when + /// certain conditions are met. Use `SpinOptions::default()` to allow the + /// Executor to keep spinning indefinitely. + pub fn spin(&mut self, options: SpinOptions) -> Result<(), RclrsError> { + loop { + if self.nodes_mtx.lock().unwrap().is_empty() { + // Nothing to spin for, so just quit here + return Ok(()); + } - /// Add a node to the executor. - pub fn add_node(&self, node: &Arc) -> Result<(), RclrsError> { - { self.nodes_mtx.lock().unwrap() }.push(Arc::downgrade(node)); - Ok(()) - } + self.spin_once(options.timeout)?; - /// Remove a node from the executor. - pub fn remove_node(&self, node: Arc) -> Result<(), RclrsError> { - { self.nodes_mtx.lock().unwrap() } - .retain(|n| !n.upgrade().map(|n| Arc::ptr_eq(&n, &node)).unwrap_or(false)); - Ok(()) + if options.only_next_available_work { + // We were only suppposed to spin once, so quit here + return Ok(()); + } + + std::thread::yield_now(); + } } /// Polls the nodes for new messages and executes the corresponding callbacks. /// /// This function additionally checks that the context is still valid. - pub fn spin_once(&self, timeout: Option) -> Result<(), RclrsError> { + fn spin_once(&self, timeout: Option) -> Result<(), RclrsError> { for node in { self.nodes_mtx.lock().unwrap() } .iter() - .filter_map(Weak::upgrade) + .filter_map(WeakNode::upgrade) .filter(|node| unsafe { rcl_context_is_valid(&*node.handle.context_handle.rcl_context.lock().unwrap()) }) @@ -66,19 +79,51 @@ impl SingleThreadedExecutor { Ok(()) } - /// Convenience function for calling [`SingleThreadedExecutor::spin_once`] in a loop. - pub fn spin(&self) -> Result<(), RclrsError> { - while !{ self.nodes_mtx.lock().unwrap() }.is_empty() { - match self.spin_once(None) { - Ok(_) - | Err(RclrsError::RclError { - code: RclReturnCode::Timeout, - .. - }) => std::thread::yield_now(), - error => return error, - } + /// Used by [`Context`] to create the `Executor`. Users cannot call this + /// function. + pub(crate) fn new(context: Arc) -> Self { + Self { + context, + nodes_mtx: Mutex::new(Vec::new()), } + } +} - Ok(()) +/// A bundle of optional conditions that a user may want to impose on how long +/// an executor spins for. +/// +/// By default the executor will be allowed to spin indefinitely. +#[non_exhaustive] +#[derive(Default)] +pub struct SpinOptions { + /// Only perform the next available work. This is similar to spin_once in + /// rclcpp and rclpy. + /// + /// To only process work that is immediately available without waiting at all, + /// set a timeout of zero. + pub only_next_available_work: bool, + /// Stop waiting after this duration of time has passed. Use `Some(0)` to not + /// wait any amount of time. Use `None` to wait an infinite amount of time. + pub timeout: Option, +} + +impl SpinOptions { + /// Use default spin options. + pub fn new() -> Self { + Self::default() + } + + /// Behave like spin_once in rclcpp and rclpy. + pub fn spin_once() -> Self { + Self { + only_next_available_work: true, + ..Default::default() + } + } + + /// Stop spinning once this durtion of time is reached. + pub fn timeout(mut self, timeout: Duration) -> Self { + self.timeout = Some(timeout); + self } } diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 4924b36cb..f41b61700 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -30,8 +30,6 @@ mod rcl_bindings; #[cfg(feature = "dyn_msg")] pub mod dynamic_message; -use std::{sync::Arc, time::Duration}; - pub use arguments::*; pub use client::*; pub use clock::*; @@ -48,66 +46,3 @@ pub use subscription::*; pub use time::*; use time_source::*; pub use wait::*; - -/// Polls the node for new messages and executes the corresponding callbacks. -/// -/// See [`WaitSet::wait`] for the meaning of the `timeout` parameter. -/// -/// This may under some circumstances return -/// [`SubscriptionTakeFailed`][1], [`ClientTakeFailed`][1], [`ServiceTakeFailed`][1] when the wait -/// set spuriously wakes up. -/// This can usually be ignored. -/// -/// [1]: crate::RclReturnCode -pub fn spin_once(node: Arc, timeout: Option) -> Result<(), RclrsError> { - let executor = SingleThreadedExecutor::new(); - executor.add_node(&node)?; - executor.spin_once(timeout) -} - -/// Convenience function for calling [`spin_once`] in a loop. -pub fn spin(node: Arc) -> Result<(), RclrsError> { - let executor = SingleThreadedExecutor::new(); - executor.add_node(&node)?; - executor.spin() -} - -/// Creates a new node in the empty namespace. -/// -/// Convenience function equivalent to [`Node::new`][1]. -/// Please see that function's documentation. -/// -/// [1]: crate::Node::new -/// -/// # Example -/// ``` -/// # use rclrs::{Context, RclrsError}; -/// let ctx = Context::new([])?; -/// let node = rclrs::create_node(&ctx, "my_node"); -/// assert!(node.is_ok()); -/// # Ok::<(), RclrsError>(()) -/// ``` -pub fn create_node(context: &Context, node_name: &str) -> Result, RclrsError> { - Node::new(context, node_name) -} - -/// Creates a [`NodeBuilder`]. -/// -/// Convenience function equivalent to [`NodeBuilder::new()`][1] and [`Node::builder()`][2]. -/// Please see that function's documentation. -/// -/// [1]: crate::NodeBuilder::new -/// [2]: crate::Node::builder -/// -/// # Example -/// ``` -/// # use rclrs::{Context, RclrsError}; -/// let context = Context::new([])?; -/// let node_builder = rclrs::create_node_builder(&context, "my_node"); -/// let node = node_builder.build()?; -/// assert_eq!(node.name(), "my_node"); -/// # Ok::<(), RclrsError>(()) -/// ``` -pub fn create_node_builder(context: &Context, node_name: &str) -> NodeBuilder { - Node::builder(context, node_name) -} diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 97684d6bc..d91a2c859 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,4 +1,4 @@ -mod builder; +mod node_options; mod graph; use std::{ cmp::PartialEq, @@ -11,9 +11,9 @@ use std::{ use rosidl_runtime_rs::Message; -pub use self::{builder::*, graph::*}; +pub use self::{graph::*, node_options::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, + rcl_bindings::*, Client, ClientBase, Clock, ContextHandle, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, TimeSource, ENTITY_LIFECYCLE_MUTEX, @@ -58,13 +58,9 @@ unsafe impl Send for rcl_node_t {} /// [2]: https://docs.ros.org/en/rolling/How-To-Guides/Node-arguments.html /// [3]: crate::NodeBuilder::new /// [4]: crate::NodeBuilder::namespace +#[derive(Clone)] pub struct Node { - pub(crate) clients_mtx: Mutex>>, - pub(crate) guard_conditions_mtx: Mutex>>, - pub(crate) services_mtx: Mutex>>, - pub(crate) subscriptions_mtx: Mutex>>, - time_source: TimeSource, - parameter: ParameterInterface, + pub(crate) primitives: Arc, pub(crate) handle: Arc, } @@ -78,6 +74,18 @@ pub(crate) struct NodeHandle { pub(crate) context_handle: Arc, } +/// This struct manages the primitives that are associated with a [`Node`]. +/// Storing them here allows the inner state of the `Node` to be shared across +/// clones. +pub(crate) struct NodePrimitives { + pub(crate) clients_mtx: Mutex>>, + pub(crate) guard_conditions_mtx: Mutex>>, + pub(crate) services_mtx: Mutex>>, + pub(crate) subscriptions_mtx: Mutex>>, + time_source: TimeSource, + parameter: ParameterInterface, +} + impl Drop for NodeHandle { fn drop(&mut self) { let _context_lock = self.context_handle.rcl_context.lock().unwrap(); @@ -106,17 +114,9 @@ impl fmt::Debug for Node { } impl Node { - /// Creates a new node in the empty namespace. - /// - /// See [`NodeBuilder::new()`] for documentation. - #[allow(clippy::new_ret_no_self)] - pub fn new(context: &Context, node_name: &str) -> Result, RclrsError> { - Self::builder(context, node_name).build() - } - /// Returns the clock associated with this node. pub fn get_clock(&self) -> Clock { - self.time_source.get_clock() + self.primitives.time_source.get_clock() } /// Returns the name of the node. @@ -126,15 +126,15 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError}; + /// # use rclrs::{Context, InitOptions, RclrsError}; /// // Without remapping - /// let context = Context::new([])?; - /// let node = rclrs::create_node(&context, "my_node")?; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node("my_node")?; /// assert_eq!(node.name(), "my_node"); /// // With remapping /// let remapping = ["--ros-args", "-r", "__node:=your_node"].map(String::from); - /// let context_r = Context::new(remapping)?; - /// let node_r = rclrs::create_node(&context_r, "my_node")?; + /// let executor_r = Context::new(remapping, InitOptions::default())?.create_basic_executor(); + /// let node_r = executor_r.create_node("my_node")?; /// assert_eq!(node_r.name(), "your_node"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -149,18 +149,18 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError}; + /// # use rclrs::{Context, InitOptions, RclrsError, NodeOptions}; /// // Without remapping - /// let context = Context::new([])?; - /// let node = - /// rclrs::create_node_builder(&context, "my_node") - /// .namespace("/my/namespace") - /// .build()?; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node( + /// NodeOptions::new("my_node") + /// .namespace("/my/namespace") + /// )?; /// assert_eq!(node.namespace(), "/my/namespace"); /// // With remapping /// let remapping = ["--ros-args", "-r", "__ns:=/your_namespace"].map(String::from); - /// let context_r = Context::new(remapping)?; - /// let node_r = rclrs::create_node(&context_r, "my_node")?; + /// let executor_r = Context::new(remapping, InitOptions::default())?.create_basic_executor(); + /// let node_r = executor_r.create_node("my_node")?; /// assert_eq!(node_r.namespace(), "/your_namespace"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -175,12 +175,12 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError}; - /// let context = Context::new([])?; - /// let node = - /// rclrs::create_node_builder(&context, "my_node") - /// .namespace("/my/namespace") - /// .build()?; + /// # use rclrs::{Context, RclrsError, NodeOptions}; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node( + /// NodeOptions::new("my_node") + /// .namespace("/my/namespace") + /// )?; /// assert_eq!(node.fully_qualified_name(), "/my/namespace/my_node"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -206,7 +206,7 @@ impl Node { T: rosidl_runtime_rs::Service, { let client = Arc::new(Client::::new(Arc::clone(&self.handle), topic)?); - { self.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); + { self.primitives.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); Ok(client) } @@ -224,7 +224,7 @@ impl Node { Arc::clone(&self.handle.context_handle), None, )); - { self.guard_conditions_mtx.lock().unwrap() } + { self.primitives.guard_conditions_mtx.lock().unwrap() } .push(Arc::downgrade(&guard_condition) as Weak); guard_condition } @@ -246,7 +246,7 @@ impl Node { Arc::clone(&self.handle.context_handle), Some(Box::new(callback) as Box), )); - { self.guard_conditions_mtx.lock().unwrap() } + { self.primitives.guard_conditions_mtx.lock().unwrap() } .push(Arc::downgrade(&guard_condition) as Weak); guard_condition } @@ -285,7 +285,7 @@ impl Node { topic, callback, )?); - { self.services_mtx.lock().unwrap() } + { self.primitives.services_mtx.lock().unwrap() } .push(Arc::downgrade(&service) as Weak); Ok(service) } @@ -309,7 +309,7 @@ impl Node { qos, callback, )?); - { self.subscriptions_mtx.lock() } + { self.primitives.subscriptions_mtx.lock() } .unwrap() .push(Arc::downgrade(&subscription) as Weak); Ok(subscription) @@ -317,28 +317,28 @@ impl Node { /// Returns the subscriptions that have not been dropped yet. pub(crate) fn live_subscriptions(&self) -> Vec> { - { self.subscriptions_mtx.lock().unwrap() } + { self.primitives.subscriptions_mtx.lock().unwrap() } .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_clients(&self) -> Vec> { - { self.clients_mtx.lock().unwrap() } + { self.primitives.clients_mtx.lock().unwrap() } .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_guard_conditions(&self) -> Vec> { - { self.guard_conditions_mtx.lock().unwrap() } + { self.primitives.guard_conditions_mtx.lock().unwrap() } .iter() .filter_map(Weak::upgrade) .collect() } pub(crate) fn live_services(&self) -> Vec> { - { self.services_mtx.lock().unwrap() } + { self.primitives.services_mtx.lock().unwrap() } .iter() .filter_map(Weak::upgrade) .collect() @@ -347,24 +347,24 @@ impl Node { /// Returns the ROS domain ID that the node is using. /// /// The domain ID controls which nodes can send messages to each other, see the [ROS 2 concept article][1]. - /// It can be set through the `ROS_DOMAIN_ID` environment variable. + /// It can be set through the `ROS_DOMAIN_ID` environment variable or by + /// passing custom [`NodeOptions`] into [`Context::new`][2] or [`Context::from_env`][3]. /// /// [1]: https://docs.ros.org/en/rolling/Concepts/About-Domain-ID.html + /// [2]: crate::Context::new + /// [3]: crate::Context::from_env /// /// # Example /// ``` /// # use rclrs::{Context, RclrsError}; /// // Set default ROS domain ID to 10 here /// std::env::set_var("ROS_DOMAIN_ID", "10"); - /// let context = Context::new([])?; - /// let node = rclrs::create_node(&context, "domain_id_node")?; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node("domain_id_node")?; /// let domain_id = node.domain_id(); /// assert_eq!(domain_id, 10); /// # Ok::<(), RclrsError>(()) /// ``` - // TODO: If node option is supported, - // add description about this function is for getting actual domain_id - // and about override of domain_id via node option pub fn domain_id(&self) -> usize { let rcl_node = self.handle.rcl_node.lock().unwrap(); let mut domain_id: usize = 0; @@ -385,8 +385,8 @@ impl Node { /// # Example /// ``` /// # use rclrs::{Context, ParameterRange, RclrsError}; - /// let context = Context::new([])?; - /// let node = rclrs::create_node(&context, "domain_id_node")?; + /// let executor = Context::default().create_basic_executor(); + /// let node = executor.create_node("domain_id_node")?; /// // Set it to a range of 0-100, with a step of 2 /// let range = ParameterRange { /// lower: Some(0), @@ -409,36 +409,50 @@ impl Node { &'a self, name: impl Into>, ) -> ParameterBuilder<'a, T> { - self.parameter.declare(name.into()) + self.primitives.parameter.declare(name.into()) } /// Enables usage of undeclared parameters for this node. /// /// Returns a [`Parameters`] struct that can be used to get and set all parameters. pub fn use_undeclared_parameters(&self) -> Parameters { - self.parameter.allow_undeclared(); + self.primitives.parameter.allow_undeclared(); Parameters { - interface: &self.parameter, + interface: &self.primitives.parameter, } } - /// Creates a [`NodeBuilder`][1] with the given name. - /// - /// Convenience function equivalent to [`NodeBuilder::new()`][2]. - /// - /// [1]: crate::NodeBuilder - /// [2]: crate::NodeBuilder::new - /// - /// # Example - /// ``` - /// # use rclrs::{Context, Node, RclrsError}; - /// let context = Context::new([])?; - /// let node = Node::builder(&context, "my_node").build()?; - /// assert_eq!(node.name(), "my_node"); - /// # Ok::<(), RclrsError>(()) - /// ``` - pub fn builder(context: &Context, node_name: &str) -> NodeBuilder { - NodeBuilder::new(context, node_name) + /// Get a `WeakNode` for this `Node`. + pub fn downgrade(&self) -> WeakNode { + WeakNode { + primitives: Arc::downgrade(&self.primitives), + handle: Arc::downgrade(&self.handle), + } + } +} + +/// This gives weak access to a [`Node`]. +/// +/// Holding onto this will not keep the `Node` alive, but calling [`Self::upgrade`] +/// on it will give you access to the `Node` if it is still alive. +#[derive(Default)] +pub struct WeakNode { + primitives: Weak, + handle: Weak, +} + +impl WeakNode { + /// Create a `WeakNode` that is not associated with any `Node`. + pub fn new() -> WeakNode { + Self::default() + } + + /// Get the [`Node`] that this `WeakNode` refers to if it's available. + pub fn upgrade(&self) -> Option { + Some(Node { + primitives: self.primitives.upgrade()?, + handle: self.handle.upgrade()?, + }) } } diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 343fa61d3..106fac72c 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -482,11 +482,12 @@ mod tests { .map(|value: usize| if value != 99 { 99 } else { 98 }) .unwrap_or(99); - let context = - Context::new_with_options([], InitOptions::new().with_domain_id(Some(domain_id))) - .unwrap(); + let executor = + Context::new([], InitOptions::new().with_domain_id(Some(domain_id))) + .unwrap() + .create_basic_executor(); let node_name = "test_publisher_names_and_types"; - let node = Node::new(&context, node_name).unwrap(); + let node = executor.create_node(node_name).unwrap(); // Test that the graph has no publishers let names_and_topics = node .get_publisher_names_and_types_by_node(node_name, "") @@ -543,9 +544,9 @@ mod tests { #[test] fn test_node_names() { - let context = Context::new([]).unwrap(); + let executor = Context::default().create_basic_executor(); let node_name = "test_node_names"; - let node = Node::new(&context, node_name).unwrap(); + let node = executor.create_node(node_name).unwrap(); let names_and_namespaces = node.get_node_names().unwrap(); @@ -559,9 +560,9 @@ mod tests { #[test] fn test_node_names_with_enclaves() { - let context = Context::new([]).unwrap(); + let executor = Context::default().create_basic_executor(); let node_name = "test_node_names_with_enclaves"; - let node = Node::new(&context, node_name).unwrap(); + let node = executor.create_node(node_name).unwrap(); let names_and_namespaces = node.get_node_names_with_enclaves().unwrap(); diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/node_options.rs similarity index 75% rename from rclrs/src/node/builder.rs rename to rclrs/src/node/node_options.rs index 011d5d2f3..801385bcf 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/node_options.rs @@ -4,7 +4,8 @@ use std::{ }; use crate::{ - rcl_bindings::*, ClockType, Context, ContextHandle, Node, NodeHandle, ParameterInterface, + rcl_bindings::*, + ClockType, Node, NodeHandle, ParameterInterface, NodePrimitives, ContextHandle, QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, }; @@ -24,27 +25,26 @@ use crate::{ /// /// # Example /// ``` -/// # use rclrs::{Context, NodeBuilder, Node, RclrsError}; -/// let context = Context::new([])?; +/// # use rclrs::{Context, NodeOptions, Node, RclrsError}; +/// let executor = Context::default().create_basic_executor(); /// // Building a node in a single expression -/// let node = NodeBuilder::new(&context, "foo_node").namespace("/bar").build()?; +/// let node = executor.create_node(NodeOptions::new("foo_node").namespace("/bar"))?; /// assert_eq!(node.name(), "foo_node"); /// assert_eq!(node.namespace(), "/bar"); -/// // Building a node via Node::builder() -/// let node = Node::builder(&context, "bar_node").build()?; +/// // Building a node via NodeOptions +/// let node = executor.create_node(NodeOptions::new("bar_node"))?; /// assert_eq!(node.name(), "bar_node"); /// // Building a node step-by-step -/// let mut builder = Node::builder(&context, "goose"); -/// builder = builder.namespace("/duck/duck"); -/// let node = builder.build()?; +/// let mut options = NodeOptions::new("goose"); +/// options = options.namespace("/duck/duck"); +/// let node = executor.create_node(options)?; /// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose"); /// # Ok::<(), RclrsError>(()) /// ``` /// /// [1]: crate::Node /// [2]: crate::Node::builder -pub struct NodeBuilder { - context: Arc, +pub struct NodeOptions { name: String, namespace: String, use_global_arguments: bool, @@ -55,7 +55,7 @@ pub struct NodeBuilder { clock_qos: QoSProfile, } -impl NodeBuilder { +impl NodeOptions { /// Creates a builder for a node with the given name. /// /// See the [`Node` docs][1] for general information on node names. @@ -72,17 +72,15 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, NodeBuilder, RclrsError, RclReturnCode}; - /// let context = Context::new([])?; + /// # use rclrs::{Context, NodeOptions, RclrsError, RclReturnCode}; + /// let executor = Context::default().create_basic_executor(); /// // This is a valid node name - /// assert!(NodeBuilder::new(&context, "my_node").build().is_ok()); + /// assert!(executor.create_node(NodeOptions::new("my_node")).is_ok()); /// // This is another valid node name (although not a good one) - /// assert!(NodeBuilder::new(&context, "_______").build().is_ok()); + /// assert!(executor.create_node(NodeOptions::new("_______")).is_ok()); /// // This is an invalid node name /// assert!(matches!( - /// NodeBuilder::new(&context, "röböt") - /// .build() - /// .unwrap_err(), + /// executor.create_node(NodeOptions::new("röböt")).unwrap_err(), /// RclrsError::RclError { code: RclReturnCode::NodeInvalidName, .. } /// )); /// # Ok::<(), RclrsError>(()) @@ -91,9 +89,8 @@ impl NodeBuilder { /// [1]: crate::Node#naming /// [2]: https://docs.ros2.org/latest/api/rmw/validate__node__name_8h.html#a5690a285aed9735f89ef11950b6e39e3 /// [3]: NodeBuilder::build - pub fn new(context: &Context, name: &str) -> NodeBuilder { - NodeBuilder { - context: Arc::clone(&context.handle), + pub fn new(name: impl ToString) -> NodeOptions { + NodeOptions { name: name.to_string(), namespace: "/".to_string(), use_global_arguments: true, @@ -126,25 +123,25 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, RclrsError, RclReturnCode}; - /// let context = Context::new([])?; + /// # use rclrs::{Context, Node, NodeOptions, RclrsError, RclReturnCode}; + /// let executor = Context::default().create_basic_executor(); /// // This is a valid namespace - /// let builder_ok_ns = Node::builder(&context, "my_node").namespace("/some/nested/namespace"); - /// assert!(builder_ok_ns.build().is_ok()); + /// let options_ok_ns = NodeOptions::new("my_node").namespace("/some/nested/namespace"); + /// assert!(executor.create_node(options_ok_ns).is_ok()); /// // This is an invalid namespace /// assert!(matches!( - /// Node::builder(&context, "my_node") + /// executor.create_node( + /// NodeOptions::new("my_node") /// .namespace("/10_percent_luck/20_percent_skill") - /// .build() - /// .unwrap_err(), + /// ).unwrap_err(), /// RclrsError::RclError { code: RclReturnCode::NodeInvalidNamespace, .. } /// )); /// // A missing forward slash at the beginning is automatically added /// assert_eq!( - /// Node::builder(&context, "my_node") + /// executor.create_node( + /// NodeOptions::new("my_node") /// .namespace("foo") - /// .build()? - /// .namespace(), + /// )?.namespace(), /// "/foo" /// ); /// # Ok::<(), RclrsError>(()) @@ -154,7 +151,7 @@ impl NodeBuilder { /// [2]: http://design.ros2.org/articles/topic_and_service_names.html /// [3]: https://docs.ros2.org/latest/api/rmw/validate__namespace_8h.html#a043f17d240cf13df01321b19a469ee49 /// [4]: NodeBuilder::build - pub fn namespace(mut self, namespace: &str) -> Self { + pub fn namespace(mut self, namespace: impl ToString) -> Self { self.namespace = namespace.to_string(); self } @@ -165,21 +162,21 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, NodeBuilder, RclrsError}; + /// # use rclrs::{Context, InitOptions, Node, NodeOptions, RclrsError}; /// let context_args = ["--ros-args", "--remap", "__node:=your_node"] /// .map(String::from); - /// let context = Context::new(context_args)?; + /// let executor = Context::new(context_args, InitOptions::default())?.create_basic_executor(); /// // Ignore the global arguments: - /// let node_without_global_args = - /// rclrs::create_node_builder(&context, "my_node") - /// .use_global_arguments(false) - /// .build()?; + /// let node_without_global_args = executor.create_node( + /// NodeOptions::new("my_node") + /// .use_global_arguments(false) + /// )?; /// assert_eq!(node_without_global_args.name(), "my_node"); /// // Do not ignore the global arguments: - /// let node_with_global_args = - /// rclrs::create_node_builder(&context, "my_other_node") - /// .use_global_arguments(true) - /// .build()?; + /// let node_with_global_args = executor.create_node( + /// NodeOptions::new("my_other_node") + /// .use_global_arguments(true) + /// )?; /// assert_eq!(node_with_global_args.name(), "your_node"); /// # Ok::<(), RclrsError>(()) /// ``` @@ -200,26 +197,29 @@ impl NodeBuilder { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, NodeBuilder, RclrsError}; + /// # use rclrs::{Context, InitOptions, Node, NodeOptions, RclrsError}; /// // Usually, this would change the name of "my_node" to "context_args_node": /// let context_args = ["--ros-args", "--remap", "my_node:__node:=context_args_node"] /// .map(String::from); - /// let context = Context::new(context_args)?; + /// let executor = Context::new(context_args, InitOptions::default())?.create_basic_executor(); /// // But the node arguments will change it to "node_args_node": /// let node_args = ["--ros-args", "--remap", "my_node:__node:=node_args_node"] /// .map(String::from); - /// let node = - /// rclrs::create_node_builder(&context, "my_node") - /// .arguments(node_args) - /// .build()?; + /// let node = executor.create_node( + /// NodeOptions::new("my_node") + /// .arguments(node_args) + /// )?; /// assert_eq!(node.name(), "node_args_node"); /// # Ok::<(), RclrsError>(()) /// ``` /// /// [1]: crate::Context::new /// [2]: https://design.ros2.org/articles/ros_command_line_arguments.html - pub fn arguments(mut self, arguments: impl IntoIterator) -> Self { - self.arguments = arguments.into_iter().collect(); + pub fn arguments(mut self, arguments: Args) -> Self + where + Args::Item: ToString, + { + self.arguments = arguments.into_iter().map(|item| item.to_string()).collect(); self } @@ -257,12 +257,12 @@ impl NodeBuilder { /// Builds the node instance. /// - /// Node name and namespace validation is performed in this method. - /// - /// For example usage, see the [`NodeBuilder`][1] docs. - /// - /// [1]: crate::NodeBuilder - pub fn build(&self) -> Result, RclrsError> { + /// Only used internally. Downstream users should call + /// [`Executor::create_node`]. + pub(crate) fn build( + self, + context: &Arc, + ) -> Result { let node_name = CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul { err, @@ -274,7 +274,7 @@ impl NodeBuilder { s: self.namespace.clone(), })?; let rcl_node_options = self.create_rcl_node_options()?; - let rcl_context = &mut *self.context.rcl_context.lock().unwrap(); + let rcl_context = &mut *context.rcl_context.lock().unwrap(); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_node = unsafe { rcl_get_zero_initialized_node() }; @@ -298,7 +298,7 @@ impl NodeBuilder { let handle = Arc::new(NodeHandle { rcl_node: Mutex::new(rcl_node), - context_handle: Arc::clone(&self.context), + context_handle: Arc::clone(&context), }); let parameter = { let rcl_node = handle.rcl_node.lock().unwrap(); @@ -308,21 +308,26 @@ impl NodeBuilder { &rcl_context.global_arguments, )? }; - let node = Arc::new(Node { + + let node = Node { handle, - clients_mtx: Mutex::new(vec![]), - guard_conditions_mtx: Mutex::new(vec![]), - services_mtx: Mutex::new(vec![]), - subscriptions_mtx: Mutex::new(vec![]), - time_source: TimeSource::builder(self.clock_type) - .clock_qos(self.clock_qos) - .build(), - parameter, - }); - node.time_source.attach_node(&node); + primitives: Arc::new(NodePrimitives { + clients_mtx: Mutex::default(), + guard_conditions_mtx: Mutex::default(), + services_mtx: Mutex::default(), + subscriptions_mtx: Mutex::default(), + time_source: TimeSource::builder(self.clock_type) + .clock_qos(self.clock_qos) + .build(), + parameter, + }), + }; + node.primitives.time_source.attach_node(&node); + if self.start_parameter_services { - node.parameter.create_services(&node)?; + node.primitives.parameter.create_services(&node)?; } + Ok(node) } @@ -367,6 +372,12 @@ impl NodeBuilder { } } +impl From for NodeOptions { + fn from(name: T) -> Self { + NodeOptions::new(name) + } +} + impl Drop for rcl_node_options_t { fn drop(&mut self) { // SAFETY: Do not finish this struct except here. diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index c7153ce01..5be93adab 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -871,18 +871,23 @@ impl ParameterInterface { #[cfg(test)] mod tests { use super::*; - use crate::{create_node, Context}; + use crate::{Context, InitOptions}; #[test] fn test_parameter_override_errors() { // Create a new node with a few parameter overrides - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("declared_int:=10"), - ]) - .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor.create_node("param_test_node").unwrap(); // Declaring a parameter with a different type than what was overridden should return an // error @@ -928,19 +933,24 @@ mod tests { #[test] fn test_parameter_setting_declaring() { // Create a new node with a few parameter overrides - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("declared_int:=10"), - String::from("-p"), - String::from("double_array:=[1.0, 2.0]"), - String::from("-p"), - String::from("optional_bool:=true"), - String::from("-p"), - String::from("non_declared_string:='param'"), - ]) - .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + String::from("-p"), + String::from("double_array:=[1.0, 2.0]"), + String::from("-p"), + String::from("optional_bool:=true"), + String::from("-p"), + String::from("non_declared_string:='param'"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor.create_node("param_test_node").unwrap(); let overridden_int = node .declare_parameter("declared_int") @@ -1084,13 +1094,18 @@ mod tests { #[test] fn test_override_undeclared_set_priority() { - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("declared_int:=10"), - ]) - .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor.create_node("param_test_node").unwrap(); // If a parameter was set as an override and as an undeclared parameter, the undeclared // value should get priority node.use_undeclared_parameters() @@ -1106,13 +1121,18 @@ mod tests { #[test] fn test_parameter_scope_redeclaring() { - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("declared_int:=10"), - ]) - .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("declared_int:=10"), + ], + InitOptions::default(), + ) + .unwrap() + .create_basic_executor(); + + let node = executor.create_node("param_test_node").unwrap(); { // Setting a parameter with an override let param = node @@ -1157,8 +1177,10 @@ mod tests { #[test] fn test_parameter_ranges() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node("param_test_node") + .unwrap(); // Setting invalid ranges should fail let range = ParameterRange { lower: Some(10), @@ -1285,8 +1307,10 @@ mod tests { #[test] fn test_readonly_parameters() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node("param_test_node") + .unwrap(); let param = node .declare_parameter("int_param") .default(100) @@ -1312,8 +1336,10 @@ mod tests { #[test] fn test_preexisting_value_error() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node("param_test_node") + .unwrap(); node.use_undeclared_parameters() .set("int_param", 100) .unwrap(); @@ -1365,8 +1391,10 @@ mod tests { #[test] fn test_optional_parameter_apis() { - let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node("param_test_node") + .unwrap(); node.declare_parameter::("int_param") .optional() .unwrap(); diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 7c8ffe62d..11df07f28 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -312,23 +312,26 @@ mod tests { }, srv::rmw::*, }, - Context, MandatoryParameter, Node, NodeBuilder, ParameterRange, ParameterValue, RclrsError, - ReadOnlyParameter, + Context, MandatoryParameter, Node, ParameterRange, ParameterValue, RclrsError, + ReadOnlyParameter, NodeOptions, Executor, SpinOptions, RclrsErrorFilter, }; use rosidl_runtime_rs::{seq, Sequence}; - use std::sync::{Arc, RwLock}; + use std::{ + sync::{Arc, RwLock}, + time::Duration, + }; struct TestNode { - node: Arc, + node: Node, bool_param: MandatoryParameter, _ns_param: MandatoryParameter, _read_only_param: ReadOnlyParameter, dynamic_param: MandatoryParameter, } - async fn try_until_timeout(f: F) -> Result<(), ()> + async fn try_until_timeout(mut f: F) -> Result<(), ()> where - F: FnOnce() -> bool + Copy, + F: FnMut() -> bool, { let mut retry_count = 0; while !f() { @@ -341,11 +344,13 @@ mod tests { Ok(()) } - fn construct_test_nodes(context: &Context, ns: &str) -> (TestNode, Arc) { - let node = NodeBuilder::new(context, "node") + fn construct_test_nodes(ns: &str) -> (Executor, TestNode, Node) { + let executor = Context::default().create_basic_executor(); + let node = executor.create_node( + NodeOptions::new("node") .namespace(ns) - .build() - .unwrap(); + ) + .unwrap(); let range = ParameterRange { lower: Some(0), upper: Some(100), @@ -375,12 +380,14 @@ mod tests { .mandatory() .unwrap(); - let client = NodeBuilder::new(context, "client") + let client = executor.create_node( + NodeOptions::new("client") .namespace(ns) - .build() - .unwrap(); + ) + .unwrap(); ( + executor, TestNode { node, bool_param, @@ -394,8 +401,7 @@ mod tests { #[test] fn test_parameter_services_names_and_types() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); - let (node, _client) = construct_test_nodes(&context, "names_types"); + let (_, node, _client) = construct_test_nodes("names_types"); std::thread::sleep(std::time::Duration::from_millis(100)); @@ -429,8 +435,7 @@ mod tests { #[tokio::test] async fn test_list_parameters_service() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); - let (node, client) = construct_test_nodes(&context, "list"); + let (mut executor, _test, client) = construct_test_nodes("list"); let list_client = client.create_client::("/list/node/list_parameters")?; try_until_timeout(|| list_client.service_is_ready().unwrap()) @@ -441,9 +446,14 @@ mod tests { let inner_done = done.clone(); let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(|| { - crate::spin_once(node.node.clone(), Some(std::time::Duration::ZERO)).ok(); - crate::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); + try_until_timeout(move || { + executor.spin( + SpinOptions::spin_once() + .timeout(Duration::ZERO) + ) + .timeout_ok() + .unwrap(); + *inner_done.read().unwrap() }) .await @@ -542,7 +552,6 @@ mod tests { move |response: ListParameters_Response| { *call_done.write().unwrap() = true; let names = response.result.names; - dbg!(&names); assert_eq!(names.len(), 2); assert_eq!(names[0].to_string(), "bool"); assert_eq!(names[1].to_string(), "use_sim_time"); @@ -564,14 +573,14 @@ mod tests { #[tokio::test] async fn test_get_set_parameters_service() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); - let (node, client) = construct_test_nodes(&context, "get_set"); + let (mut executor, test, client) = construct_test_nodes("get_set"); let get_client = client.create_client::("/get_set/node/get_parameters")?; let set_client = client.create_client::("/get_set/node/set_parameters")?; let set_atomically_client = client .create_client::("/get_set/node/set_parameters_atomically")?; try_until_timeout(|| { + println!(" >> testing services"); get_client.service_is_ready().unwrap() && set_client.service_is_ready().unwrap() && set_atomically_client.service_is_ready().unwrap() @@ -581,18 +590,26 @@ mod tests { let done = Arc::new(RwLock::new(false)); - let inner_node = node.node.clone(); let inner_done = done.clone(); let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(|| { - crate::spin_once(inner_node.clone(), Some(std::time::Duration::ZERO)).ok(); - crate::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); + try_until_timeout(move || { + println!(" -- spin"); + executor.spin( + SpinOptions::spin_once() + .timeout(Duration::ZERO) + ) + .timeout_ok() + .unwrap(); + *inner_done.read().unwrap() }) .await .unwrap(); }); + let _hold_node = test.node.clone(); + let _hold_client = client.clone(); + let res = tokio::task::spawn(async move { // Get an existing parameter let request = GetParameters_Request { @@ -636,7 +653,10 @@ mod tests { }, ) .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) + try_until_timeout(|| { + println!("checking client"); + *client_finished.read().unwrap() + }) .await .unwrap(); @@ -711,7 +731,7 @@ mod tests { let client_finished = Arc::new(RwLock::new(false)); let call_done = client_finished.clone(); // Parameter is assigned a default of true at declaration time - assert!(node.bool_param.get()); + assert!(test.bool_param.get()); set_client .async_send_request_with_callback( &request, @@ -721,14 +741,14 @@ mod tests { // Setting a bool value set for a bool parameter assert!(response.results[0].successful); // Value was set to false, node parameter get should reflect this - assert!(!node.bool_param.get()); + assert!(!test.bool_param.get()); // Setting a parameter to the wrong type assert!(!response.results[1].successful); // Setting a read only parameter assert!(!response.results[2].successful); // Setting a dynamic parameter to a new type assert!(response.results[3].successful); - assert_eq!(node.dynamic_param.get(), ParameterValue::Bool(true)); + assert_eq!(test.dynamic_param.get(), ParameterValue::Bool(true)); // Setting a value out of range assert!(!response.results[4].successful); // Setting an invalid type @@ -743,7 +763,7 @@ mod tests { .unwrap(); // Set the node to use undeclared parameters and try to set one - node.node.use_undeclared_parameters(); + test.node.use_undeclared_parameters(); let request = SetParameters_Request { parameters: seq![undeclared_bool], }; @@ -758,7 +778,7 @@ mod tests { // Setting the undeclared parameter is now allowed assert!(response.results[0].successful); assert_eq!( - node.node.use_undeclared_parameters().get("undeclared_bool"), + test.node.use_undeclared_parameters().get("undeclared_bool"), Some(ParameterValue::Bool(true)) ); }, @@ -783,7 +803,10 @@ mod tests { }, ) .unwrap(); - try_until_timeout(|| *client_finished.read().unwrap()) + try_until_timeout(|| { + println!("checking client finished"); + *client_finished.read().unwrap() + }) .await .unwrap(); *done.write().unwrap() = true; @@ -797,8 +820,7 @@ mod tests { #[tokio::test] async fn test_describe_get_types_parameters_service() -> Result<(), RclrsError> { - let context = Context::new([]).unwrap(); - let (node, client) = construct_test_nodes(&context, "describe"); + let (mut executor, _test, client) = construct_test_nodes("describe"); let describe_client = client.create_client::("/describe/node/describe_parameters")?; let get_types_client = @@ -814,11 +836,15 @@ mod tests { let done = Arc::new(RwLock::new(false)); let inner_done = done.clone(); - let inner_node = node.node.clone(); let rclrs_spin = tokio::task::spawn(async move { - try_until_timeout(|| { - crate::spin_once(inner_node.clone(), Some(std::time::Duration::ZERO)).ok(); - crate::spin_once(client.clone(), Some(std::time::Duration::ZERO)).ok(); + try_until_timeout(move || { + executor.spin( + SpinOptions::spin_once() + .timeout(Duration::ZERO) + ) + .timeout_ok() + .unwrap(); + *inner_done.read().unwrap() }) .await diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 82fe31ebb..3a20ae24b 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -537,7 +537,7 @@ impl ParameterValue { #[cfg(test)] mod tests { use super::*; - use crate::{Context, RclrsError, ToResult}; + use crate::{Context, RclrsError, ToResult, InitOptions}; // TODO(luca) tests for all from / to ParameterVariant functions @@ -565,11 +565,14 @@ mod tests { ), ]; for pair in input_output_pairs { - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - format!("foo:={}", pair.0), - ])?; + let ctx = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + format!("foo:={}", pair.0), + ], + InitOptions::default(), + )?; let mut rcl_params = std::ptr::null_mut(); unsafe { rcl_arguments_get_param_overrides( diff --git a/rclrs/src/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index 1e9b581ae..ce3e82f67 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,19 +1,20 @@ -use crate::{Context, Node, NodeBuilder, RclrsError}; -use std::sync::Arc; +use crate::{Context, Node, RclrsError, NodeOptions}; pub(crate) struct TestGraph { - pub node1: Arc, - pub node2: Arc, + pub node1: Node, + pub node2: Node, } pub(crate) fn construct_test_graph(namespace: &str) -> Result { - let context = Context::new([])?; + let executor = Context::default().create_basic_executor(); Ok(TestGraph { - node1: NodeBuilder::new(&context, "graph_test_node_1") + node1: executor.create_node( + NodeOptions::new("graph_test_node_1") .namespace(namespace) - .build()?, - node2: NodeBuilder::new(&context, "graph_test_node_2") + )?, + node2: executor.create_node( + NodeOptions::new("graph_test_node_2") .namespace(namespace) - .build()?, + )?, }) } diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index a6b600800..a5dcad50f 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -1,15 +1,15 @@ use crate::{ clock::{Clock, ClockSource, ClockType}, vendor::rosgraph_msgs::msg::Clock as ClockMsg, - Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, + Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, WeakNode, }; -use std::sync::{Arc, Mutex, RwLock, Weak}; +use std::sync::{Arc, Mutex, RwLock}; /// Time source for a node that drives the attached clock. /// If the node's `use_sim_time` parameter is set to `true`, the `TimeSource` will subscribe /// to the `/clock` topic and drive the attached clock pub(crate) struct TimeSource { - node: Mutex>, + node: Mutex, clock: RwLock, clock_source: Arc>>, requested_clock_type: ClockType, @@ -60,7 +60,7 @@ impl TimeSourceBuilder { ClockType::SteadyTime => Clock::steady(), }; TimeSource { - node: Mutex::new(Weak::new()), + node: Mutex::new(WeakNode::new()), clock: RwLock::new(clock), clock_source: Arc::new(Mutex::new(None)), requested_clock_type: self.clock_type, @@ -85,7 +85,7 @@ impl TimeSource { /// Attaches the given node to to the `TimeSource`, using its interface to read the /// `use_sim_time` parameter and create the clock subscription. - pub(crate) fn attach_node(&self, node: &Arc) { + pub(crate) fn attach_node(&self, node: &Node) { // TODO(luca) Make this parameter editable and register a parameter callback // that calls set_ros_time(bool) once parameter callbacks are implemented. let param = node @@ -93,7 +93,7 @@ impl TimeSource { .default(false) .read_only() .unwrap(); - *self.node.lock().unwrap() = Arc::downgrade(node); + *self.node.lock().unwrap() = node.downgrade(); self.set_ros_time_enable(param.get()); *self.use_sim_time.lock().unwrap() = Some(param); } @@ -145,24 +145,29 @@ impl TimeSource { #[cfg(test)] mod tests { - use crate::{create_node, Context}; + use crate::{Context, InitOptions}; #[test] fn time_source_default_clock() { - let node = create_node(&Context::new([]).unwrap(), "test_node").unwrap(); + let node = Context::default().create_basic_executor().create_node("test_node").unwrap(); // Default clock should be above 0 (use_sim_time is default false) assert!(node.get_clock().now().nsec > 0); } #[test] fn time_source_sim_time() { - let ctx = Context::new([ - String::from("--ros-args"), - String::from("-p"), - String::from("use_sim_time:=true"), - ]) - .unwrap(); - let node = create_node(&ctx, "test_node").unwrap(); + let executor = Context::new( + [ + String::from("--ros-args"), + String::from("-p"), + String::from("use_sim_time:=true"), + ], + InitOptions::default() + ) + .unwrap() + .create_basic_executor(); + + let node = executor.create_node("test_node").unwrap(); // Default sim time value should be 0 (no message received) assert_eq!(node.get_clock().now().nsec, 0); } diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 243c9d857..c0e0c659d 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -427,7 +427,7 @@ mod tests { #[test] fn guard_condition_in_wait_set_readies() -> Result<(), RclrsError> { - let context = Context::new([])?; + let context = Context::default(); let guard_condition = Arc::new(GuardCondition::new(&context)); diff --git a/rclrs/src/wait/guard_condition.rs b/rclrs/src/wait/guard_condition.rs index 92a6acd00..d9d8b62d9 100644 --- a/rclrs/src/wait/guard_condition.rs +++ b/rclrs/src/wait/guard_condition.rs @@ -16,7 +16,7 @@ use crate::{rcl_bindings::*, Context, ContextHandle, RclrsError, ToResult}; /// # use rclrs::{Context, GuardCondition, WaitSet, RclrsError}; /// # use std::sync::{Arc, atomic::Ordering}; /// -/// let context = Context::new([])?; +/// let context = Context::default(); /// /// let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false)); /// let atomic_bool_for_closure = Arc::clone(&atomic_bool); @@ -162,7 +162,7 @@ mod tests { #[test] fn test_guard_condition() -> Result<(), RclrsError> { - let context = Context::new([])?; + let context = Context::default(); let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false)); let atomic_bool_for_closure = Arc::clone(&atomic_bool); @@ -180,7 +180,7 @@ mod tests { #[test] fn test_guard_condition_wait() -> Result<(), RclrsError> { - let context = Context::new([])?; + let context = Context::default(); let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false)); let atomic_bool_for_closure = Arc::clone(&atomic_bool); From 1b5c187a761ae9a9de92392e593c45e6fd0e9554 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 16 Nov 2024 16:34:30 +0000 Subject: [PATCH 07/20] Update examples Signed-off-by: Michael X. Grey --- examples/message_demo/src/message_demo.rs | 10 +++--- .../src/minimal_client.rs | 8 ++--- .../src/minimal_client_async.rs | 8 ++--- .../src/minimal_service.rs | 8 ++--- .../minimal_pub_sub/src/minimal_publisher.rs | 7 ++-- .../minimal_pub_sub/src/minimal_subscriber.rs | 9 +++--- .../minimal_pub_sub/src/minimal_two_nodes.rs | 32 +++++++------------ .../src/zero_copy_publisher.rs | 7 ++-- .../src/zero_copy_subscriber.rs | 8 ++--- examples/rust_pubsub/src/simple_publisher.rs | 27 ++++++++-------- examples/rust_pubsub/src/simple_subscriber.rs | 17 +++++----- 11 files changed, 60 insertions(+), 81 deletions(-) diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs index 3e2bf53b2..bc3b7c028 100644 --- a/examples/message_demo/src/message_demo.rs +++ b/examples/message_demo/src/message_demo.rs @@ -1,4 +1,4 @@ -use std::{convert::TryInto, env, sync::Arc}; +use std::convert::TryInto; use anyhow::{Error, Result}; use rosidl_runtime_rs::{seq, BoundedSequence, Message, Sequence}; @@ -138,8 +138,8 @@ fn demonstrate_sequences() { fn demonstrate_pubsub() -> Result<(), Error> { println!("================== Interoperability demo =================="); // Demonstrate interoperability between idiomatic and RMW-native message types - let context = rclrs::Context::new(env::args())?; - let node = rclrs::create_node(&context, "message_demo")?; + let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); + let node = executor.create_node("message_demo")?; let idiomatic_publisher = node.create_publisher::( "topic", @@ -166,10 +166,10 @@ fn demonstrate_pubsub() -> Result<(), Error> { )?; println!("Sending idiomatic message."); idiomatic_publisher.publish(rclrs_example_msgs::msg::VariousTypes::default())?; - rclrs::spin_once(Arc::clone(&node), None)?; + executor.spin(rclrs::SpinOptions::spin_once())?; println!("Sending RMW-native message."); direct_publisher.publish(rclrs_example_msgs::msg::rmw::VariousTypes::default())?; - rclrs::spin_once(Arc::clone(&node), None)?; + executor.spin(rclrs::SpinOptions::spin_once())?; Ok(()) } diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs index 915541d54..baeff971d 100644 --- a/examples/minimal_client_service/src/minimal_client.rs +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -1,11 +1,9 @@ -use std::env; - use anyhow::{Error, Result}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_client")?; + let node = executor.create_node("minimal_client")?; let client = node.create_client::("add_two_ints")?; @@ -30,5 +28,5 @@ fn main() -> Result<(), Error> { std::thread::sleep(std::time::Duration::from_millis(500)); println!("Waiting for response"); - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) } diff --git a/examples/minimal_client_service/src/minimal_client_async.rs b/examples/minimal_client_service/src/minimal_client_async.rs index 0eeb87f4d..f011569de 100644 --- a/examples/minimal_client_service/src/minimal_client_async.rs +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -1,12 +1,10 @@ -use std::env; - use anyhow::{Error, Result}; #[tokio::main] async fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_client")?; + let node = executor.create_node("minimal_client")?; let client = node.create_client::("add_two_ints")?; @@ -22,7 +20,7 @@ async fn main() -> Result<(), Error> { println!("Waiting for response"); - let rclrs_spin = tokio::task::spawn_blocking(move || rclrs::spin(node)); + let rclrs_spin = tokio::task::spawn_blocking(move || executor.spin(rclrs::SpinOptions::default())); let response = future.await?; println!( diff --git a/examples/minimal_client_service/src/minimal_service.rs b/examples/minimal_client_service/src/minimal_service.rs index b4149c817..f38a49f21 100644 --- a/examples/minimal_client_service/src/minimal_service.rs +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -1,5 +1,3 @@ -use std::env; - use anyhow::{Error, Result}; fn handle_service( @@ -13,13 +11,13 @@ fn handle_service( } fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_service")?; + let node = executor.create_node("minimal_service")?; let _server = node .create_service::("add_two_ints", handle_service)?; println!("Starting server"); - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/minimal_publisher.rs b/examples/minimal_pub_sub/src/minimal_publisher.rs index 720086917..7ca95cf97 100644 --- a/examples/minimal_pub_sub/src/minimal_publisher.rs +++ b/examples/minimal_pub_sub/src/minimal_publisher.rs @@ -1,11 +1,10 @@ -use std::env; - use anyhow::{Error, Result}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let context = rclrs::Context::default_from_env()?; + let executor = context.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_publisher")?; + let node = executor.create_node("minimal_publisher")?; let publisher = node.create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index ebc5fc194..2d375bd7d 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -1,11 +1,10 @@ -use std::env; - use anyhow::{Error, Result}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let context = rclrs::Context::default_from_env()?; + let mut executor = context.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_subscriber")?; + let node = executor.create_node("minimal_subscriber")?; let mut num_messages: usize = 0; @@ -19,5 +18,5 @@ fn main() -> Result<(), Error> { }, )?; - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index fb03574a2..f829afc52 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -1,23 +1,19 @@ -use std::{ - env, - sync::{ - atomic::{AtomicU32, Ordering}, - Arc, Mutex, - }, +use std::sync::{ + atomic::{AtomicU32, Ordering}, + Arc, Mutex, }; use anyhow::{Error, Result}; struct MinimalSubscriber { num_messages: AtomicU32, - node: Arc, + node: rclrs::Node, subscription: Mutex>>>, } impl MinimalSubscriber { - pub fn new(name: &str, topic: &str) -> Result, rclrs::RclrsError> { - let context = rclrs::Context::new(env::args())?; - let node = rclrs::create_node(&context, name)?; + pub fn new(executor: &rclrs::Executor, name: &str, topic: &str) -> Result, rclrs::RclrsError> { + let node = executor.create_node(name)?; let minimal_subscriber = Arc::new(MinimalSubscriber { num_messages: 0.into(), node, @@ -50,11 +46,11 @@ impl MinimalSubscriber { } fn main() -> Result<(), Error> { - let publisher_context = rclrs::Context::new(env::args())?; - let publisher_node = rclrs::create_node(&publisher_context, "minimal_publisher")?; + let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); + let publisher_node = executor.create_node("minimal_publisher")?; - let subscriber_node_one = MinimalSubscriber::new("minimal_subscriber_one", "topic")?; - let subscriber_node_two = MinimalSubscriber::new("minimal_subscriber_two", "topic")?; + let _subscriber_node_one = MinimalSubscriber::new(&executor, "minimal_subscriber_one", "topic")?; + let _subscriber_node_two = MinimalSubscriber::new(&executor, "minimal_subscriber_two", "topic")?; let publisher = publisher_node .create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; @@ -71,11 +67,5 @@ fn main() -> Result<(), Error> { } }); - let executor = rclrs::SingleThreadedExecutor::new(); - - executor.add_node(&publisher_node)?; - executor.add_node(&subscriber_node_one.node)?; - executor.add_node(&subscriber_node_two.node)?; - - executor.spin().map_err(|err| err.into()) + executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/zero_copy_publisher.rs b/examples/minimal_pub_sub/src/zero_copy_publisher.rs index 5e73b5de7..3f904f673 100644 --- a/examples/minimal_pub_sub/src/zero_copy_publisher.rs +++ b/examples/minimal_pub_sub/src/zero_copy_publisher.rs @@ -1,11 +1,10 @@ -use std::env; - use anyhow::{Error, Result}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let context = rclrs::Context::default_from_env()?; + let executor = context.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_publisher")?; + let node = executor.create_node("minimal_publisher")?; let publisher = node.create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; diff --git a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs index 9551dba0e..cfb569ab2 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -1,11 +1,9 @@ -use std::env; - use anyhow::{Error, Result}; fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; + let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); - let node = rclrs::create_node(&context, "minimal_subscriber")?; + let node = executor.create_node("minimal_subscriber")?; let mut num_messages: usize = 0; @@ -19,5 +17,5 @@ fn main() -> Result<(), Error> { }, )?; - rclrs::spin(node).map_err(|err| err.into()) + executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) } diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs index 98d0e0f74..edacd0247 100644 --- a/examples/rust_pubsub/src/simple_publisher.rs +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -1,36 +1,37 @@ -use rclrs::{create_node, Context, Node, Publisher, RclrsError, QOS_PROFILE_DEFAULT}; +use rclrs::{Context, Executor, Publisher, RclrsError, SpinOptions, QOS_PROFILE_DEFAULT}; use std::{sync::Arc, thread, time::Duration}; use std_msgs::msg::String as StringMsg; -struct SimplePublisherNode { - node: Arc, - _publisher: Arc>, + +struct SimplePublisher { + publisher: Arc>, } -impl SimplePublisherNode { - fn new(context: &Context) -> Result { - let node = create_node(context, "simple_publisher").unwrap(); - let _publisher = node + +impl SimplePublisher { + fn new(executor: &Executor) -> Result { + let node = executor.create_node("simple_publisher").unwrap(); + let publisher = node .create_publisher("publish_hello", QOS_PROFILE_DEFAULT) .unwrap(); - Ok(Self { node, _publisher }) + Ok(Self { publisher }) } fn publish_data(&self, increment: i32) -> Result { let msg: StringMsg = StringMsg { data: format!("Hello World {}", increment), }; - self._publisher.publish(msg).unwrap(); + self.publisher.publish(msg).unwrap(); Ok(increment + 1_i32) } } fn main() -> Result<(), RclrsError> { - let context = Context::new(std::env::args()).unwrap(); - let publisher = Arc::new(SimplePublisherNode::new(&context).unwrap()); + let mut executor = Context::default_from_env().unwrap().create_basic_executor(); + let publisher = Arc::new(SimplePublisher::new(&executor).unwrap()); let publisher_other_thread = Arc::clone(&publisher); let mut count: i32 = 0; thread::spawn(move || loop { thread::sleep(Duration::from_millis(1000)); count = publisher_other_thread.publish_data(count).unwrap(); }); - rclrs::spin(publisher.node.clone()) + executor.spin(SpinOptions::default()) } diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index a0d02bb4c..2ba2c4e7a 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -1,19 +1,19 @@ -use rclrs::{create_node, Context, Node, RclrsError, Subscription, QOS_PROFILE_DEFAULT}; +use rclrs::{Context, Executor, RclrsError, SpinOptions, Subscription, QOS_PROFILE_DEFAULT}; use std::{ - env, sync::{Arc, Mutex}, thread, time::Duration, }; use std_msgs::msg::String as StringMsg; + pub struct SimpleSubscriptionNode { - node: Arc, _subscriber: Arc>, data: Arc>>, } + impl SimpleSubscriptionNode { - fn new(context: &Context) -> Result { - let node = create_node(context, "simple_subscription").unwrap(); + fn new(executor: &Executor) -> Result { + let node = executor.create_node("simple_subscription").unwrap(); let data: Arc>> = Arc::new(Mutex::new(None)); let data_mut: Arc>> = Arc::clone(&data); let _subscriber = node @@ -26,7 +26,6 @@ impl SimpleSubscriptionNode { ) .unwrap(); Ok(Self { - node, _subscriber, data, }) @@ -41,12 +40,12 @@ impl SimpleSubscriptionNode { } } fn main() -> Result<(), RclrsError> { - let context = Context::new(env::args()).unwrap(); - let subscription = Arc::new(SimpleSubscriptionNode::new(&context).unwrap()); + let mut executor = Context::default_from_env().unwrap().create_basic_executor(); + let subscription = Arc::new(SimpleSubscriptionNode::new(&executor).unwrap()); let subscription_other_thread = Arc::clone(&subscription); thread::spawn(move || loop { thread::sleep(Duration::from_millis(1000)); subscription_other_thread.data_callback().unwrap() }); - rclrs::spin(subscription.node.clone()) + executor.spin(SpinOptions::default()) } From 1ec9f10acfcaffac0da6237d8aa6c6696332de57 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 19 Nov 2024 16:41:58 +0800 Subject: [PATCH 08/20] Fix documentation Signed-off-by: Michael X. Grey --- rclrs/src/context.rs | 10 ++++----- rclrs/src/node.rs | 38 ++++++++++++++++++---------------- rclrs/src/node/node_options.rs | 14 ++++++------- rclrs/src/publisher.rs | 4 ++-- rclrs/src/subscription.rs | 5 ++--- 5 files changed, 35 insertions(+), 36 deletions(-) diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index b218fc24b..5d50d1d8d 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -83,13 +83,13 @@ impl Context { /// Creates a new context. /// /// * `args` - A sequence of strings that resembles command line arguments - /// that users can pass into a ROS executable. See [the official tutorial][1] - /// to know what these arguments may look like. To simply pass in the arguments - /// that the user has provided from the command line, call [`Self::from_env`] - /// or [`Self::default_from_env`] instead. + /// that users can pass into a ROS executable. See [the official tutorial][1] + /// to know what these arguments may look like. To simply pass in the arguments + /// that the user has provided from the command line, call [`Self::from_env`] + /// or [`Self::default_from_env`] instead. /// /// * `options` - Additional options that your application can use to override - /// settings that would otherwise be determined by the environment. + /// settings that would otherwise be determined by the environment. /// /// Creating a context will fail if `args` contains invalid ROS arguments. /// diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index d91a2c859..f23a17d6d 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -28,9 +28,13 @@ unsafe impl Send for rcl_node_t {} /// Nodes are a core concept in ROS 2. Refer to the official ["Understanding ROS 2 nodes"][1] /// tutorial for an introduction. /// -/// Ownership of the node is shared with all [`Publisher`]s and [`Subscription`]s created from it. -/// That means that even after the node itself is dropped, it will continue to exist and be -/// displayed by e.g. `ros2 topic` as long as its publishers and subscriptions are not dropped. +/// Ownership of the node is shared with all the primitives such as [`Publisher`]s and [`Subscription`]s +/// that are created from it. That means that even after the `Node` itself is dropped, it will continue +/// to exist and be displayed by e.g. `ros2 topic` as long as any one of its primitives is not dropped. +/// +/// # Creating +/// Use [`Executor::create_node`][7] to create a new node. Pass in [`NodeOptions`] to set all the different +/// options for node creation, or just pass in a string for the node's name if the default options are okay. /// /// # Naming /// A node has a *name* and a *namespace*. @@ -48,16 +52,19 @@ unsafe impl Send for rcl_node_t {} /// In that sense, the parameters to the node creation functions are only the _default_ namespace and /// name. /// See also the [official tutorial][1] on the command line arguments for ROS nodes, and the -/// [`Node::namespace()`] and [`Node::name()`] functions for examples. +/// [`Node::namespace()`][3] and [`Node::name()`][4] functions for examples. /// /// ## Rules for valid names /// The rules for valid node names and node namespaces are explained in -/// [`NodeBuilder::new()`][3] and [`NodeBuilder::namespace()`][4]. +/// [`NodeOptions::new()`][5] and [`NodeOptions::namespace()`][6]. /// /// [1]: https://docs.ros.org/en/rolling/Tutorials/Understanding-ROS2-Nodes.html /// [2]: https://docs.ros.org/en/rolling/How-To-Guides/Node-arguments.html -/// [3]: crate::NodeBuilder::new -/// [4]: crate::NodeBuilder::namespace +/// [3]: Node::namespace +/// [4]: Node::name +/// [5]: crate::NodeOptions::new +/// [6]: crate::NodeOptions::namespace +/// [7]: crate::Executor::create_node #[derive(Clone)] pub struct Node { pub(crate) primitives: Arc, @@ -213,12 +220,11 @@ impl Node { /// Creates a [`GuardCondition`][1] with no callback. /// /// A weak pointer to the `GuardCondition` is stored within this node. - /// When this node is added to a wait set (e.g. when calling `spin_once`[2] - /// with this node as an argument), the guard condition can be used to - /// interrupt the wait. + /// When this node is added to a wait set (e.g. when its executor is [spinning][2]), + /// the guard condition can be used to interrupt the wait. /// /// [1]: crate::GuardCondition - /// [2]: crate::spin_once + /// [2]: crate::Executor::spin pub fn create_guard_condition(&self) -> Arc { let guard_condition = Arc::new(GuardCondition::new_with_context_handle( Arc::clone(&self.handle.context_handle), @@ -232,12 +238,11 @@ impl Node { /// Creates a [`GuardCondition`][1] with a callback. /// /// A weak pointer to the `GuardCondition` is stored within this node. - /// When this node is added to a wait set (e.g. when calling `spin_once`[2] - /// with this node as an argument), the guard condition can be used to - /// interrupt the wait. + /// When this node is added to a wait set (e.g. when its executor is [spinning][2]), + /// the guard condition can be used to interrupt the wait. /// /// [1]: crate::GuardCondition - /// [2]: crate::spin_once + /// [2]: crate::Executor::spin pub fn create_guard_condition_with_callback(&mut self, callback: F) -> Arc where F: Fn() + Send + Sync + 'static, @@ -254,7 +259,6 @@ impl Node { /// Creates a [`Publisher`][1]. /// /// [1]: crate::Publisher - // TODO: make publisher's lifetime depend on node's lifetime pub fn create_publisher( &self, topic: &str, @@ -270,7 +274,6 @@ impl Node { /// Creates a [`Service`][1]. /// /// [1]: crate::Service - // TODO: make service's lifetime depend on node's lifetime pub fn create_service( &self, topic: &str, @@ -293,7 +296,6 @@ impl Node { /// Creates a [`Subscription`][1]. /// /// [1]: crate::Subscription - // TODO: make subscription's lifetime depend on node's lifetime pub fn create_subscription( &self, topic: &str, diff --git a/rclrs/src/node/node_options.rs b/rclrs/src/node/node_options.rs index 801385bcf..e4172719d 100644 --- a/rclrs/src/node/node_options.rs +++ b/rclrs/src/node/node_options.rs @@ -9,10 +9,9 @@ use crate::{ QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, }; -/// A builder for creating a [`Node`][1]. +/// A set of options for creating a [`Node`][1]. /// /// The builder pattern allows selectively setting some fields, and leaving all others at their default values. -/// This struct instance can be created via [`Node::builder()`][2]. /// /// The default values for optional fields are: /// - `namespace: "/"` @@ -43,7 +42,6 @@ use crate::{ /// ``` /// /// [1]: crate::Node -/// [2]: crate::Node::builder pub struct NodeOptions { name: String, namespace: String, @@ -68,7 +66,7 @@ impl NodeOptions { /// - Must not be empty and not be longer than `RMW_NODE_NAME_MAX_NAME_LENGTH` /// - Must not start with a number /// - /// Note that node name validation is delayed until [`NodeBuilder::build()`][3]. + /// Note that node name validation is delayed until [`Executor::create_node`][3]. /// /// # Example /// ``` @@ -88,7 +86,7 @@ impl NodeOptions { /// /// [1]: crate::Node#naming /// [2]: https://docs.ros2.org/latest/api/rmw/validate__node__name_8h.html#a5690a285aed9735f89ef11950b6e39e3 - /// [3]: NodeBuilder::build + /// [3]: crate::Executor::create_node pub fn new(name: impl ToString) -> NodeOptions { NodeOptions { name: name.to_string(), @@ -119,7 +117,7 @@ impl NodeOptions { /// - Must not contain two or more `/` characters in a row /// - Must not have a `/` character at the end, except if `/` is the full namespace /// - /// Note that namespace validation is delayed until [`NodeBuilder::build()`][4]. + /// Note that namespace validation is delayed until [`Executor::create_node`][4]. /// /// # Example /// ``` @@ -150,7 +148,7 @@ impl NodeOptions { /// [1]: crate::Node#naming /// [2]: http://design.ros2.org/articles/topic_and_service_names.html /// [3]: https://docs.ros2.org/latest/api/rmw/validate__namespace_8h.html#a043f17d240cf13df01321b19a469ee49 - /// [4]: NodeBuilder::build + /// [4]: crate::Executor::create_node pub fn namespace(mut self, namespace: impl ToString) -> Self { self.namespace = namespace.to_string(); self @@ -298,7 +296,7 @@ impl NodeOptions { let handle = Arc::new(NodeHandle { rcl_node: Mutex::new(rcl_node), - context_handle: Arc::clone(&context), + context_handle: Arc::clone(context), }); let parameter = { let rcl_node = handle.rcl_node.lock().unwrap(); diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 2935ca322..125361d0c 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -50,9 +50,9 @@ impl Drop for PublisherHandle { /// The underlying RMW will decide on the concrete delivery mechanism (network stack, shared /// memory, or intraprocess). /// -/// Sending messages does not require calling [`spin`][1] on the publisher's node. +/// Sending messages does not require the node's executor to [spin][2]. /// -/// [1]: crate::spin +/// [2]: crate::Executor::spin pub struct Publisher where T: Message, diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index fbd518c21..645f9019c 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -66,7 +66,7 @@ pub trait SubscriptionBase: Send + Sync { /// /// There can be multiple subscriptions for the same topic, in different nodes or the same node. /// -/// Receiving messages requires calling [`spin_once`][1] or [`spin`][2] on the subscription's node. +/// Receiving messages requires the node's executor to [spin][2]. /// /// When a subscription is created, it may take some time to get "matched" with a corresponding /// publisher. @@ -74,8 +74,7 @@ pub trait SubscriptionBase: Send + Sync { /// The only available way to instantiate subscriptions is via [`Node::create_subscription()`][3], this /// is to ensure that [`Node`][4]s can track all the subscriptions that have been created. /// -/// [1]: crate::spin_once -/// [2]: crate::spin +/// [2]: crate::Executor::spin /// [3]: crate::Node::create_subscription /// [4]: crate::Node pub struct Subscription From 0874d8d4bf822075d076294c14b0b4e46416b7d2 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 15:50:51 +0800 Subject: [PATCH 09/20] Fix formatting Signed-off-by: Michael X. Grey --- rclrs/src/context.rs | 5 +-- rclrs/src/error.rs | 8 +++- rclrs/src/executor.rs | 9 ++-- rclrs/src/node.rs | 13 +++--- rclrs/src/node/graph.rs | 3 +- rclrs/src/node/node_options.rs | 11 ++--- rclrs/src/parameter/service.rs | 58 ++++++++++--------------- rclrs/src/parameter/value.rs | 2 +- rclrs/src/test_helpers/graph_helpers.rs | 12 ++--- rclrs/src/time_source.rs | 9 ++-- 10 files changed, 58 insertions(+), 72 deletions(-) diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 5d50d1d8d..17894ca46 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -6,7 +6,7 @@ use std::{ vec::Vec, }; -use crate::{rcl_bindings::*, RclrsError, ToResult, Executor}; +use crate::{rcl_bindings::*, Executor, RclrsError, ToResult}; /// This is locked whenever initializing or dropping any middleware entity /// because we have found issues in RCL and some RMW implementations that @@ -74,8 +74,7 @@ impl Default for Context { fn default() -> Self { // SAFETY: It should always be valid to instantiate a context with no // arguments, no parameters, no options, etc. - Self::new([], InitOptions::default()) - .expect("Failed to instantiate a default context") + Self::new([], InitOptions::default()).expect("Failed to instantiate a default context") } } diff --git a/rclrs/src/error.rs b/rclrs/src/error.rs index 515a5b95c..961e7d5e6 100644 --- a/rclrs/src/error.rs +++ b/rclrs/src/error.rs @@ -364,7 +364,13 @@ impl RclrsErrorFilter for Result<(), RclrsError> { match self { Ok(()) => Ok(()), Err(err) => { - if matches!(err, RclrsError::RclError { code: RclReturnCode::Timeout, .. }) { + if matches!( + err, + RclrsError::RclError { + code: RclReturnCode::Timeout, + .. + } + ) { return Ok(()); } diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 63951afef..3202bd34f 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,6 +1,6 @@ use crate::{ - rcl_bindings::rcl_context_is_valid, - Node, RclrsError, WaitSet, ContextHandle, NodeOptions, WeakNode, + rcl_bindings::rcl_context_is_valid, ContextHandle, Node, NodeOptions, RclrsError, WaitSet, + WeakNode, }; use std::{ sync::{Arc, Mutex}, @@ -15,10 +15,7 @@ pub struct Executor { impl Executor { /// Create a [`Node`] that will run on this Executor. - pub fn create_node( - &self, - options: impl Into, - ) -> Result { + pub fn create_node(&self, options: impl Into) -> Result { let options: NodeOptions = options.into(); let node = options.build(&self.context)?; self.nodes_mtx.lock().unwrap().push(node.downgrade()); diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index f23a17d6d..42cb3a226 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -1,5 +1,5 @@ -mod node_options; mod graph; +mod node_options; use std::{ cmp::PartialEq, ffi::CStr, @@ -13,10 +13,10 @@ use rosidl_runtime_rs::Message; pub use self::{graph::*, node_options::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, ContextHandle, GuardCondition, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - TimeSource, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, Client, ClientBase, Clock, ContextHandle, GuardCondition, ParameterBuilder, + ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, + ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, TimeSource, + ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -213,7 +213,8 @@ impl Node { T: rosidl_runtime_rs::Service, { let client = Arc::new(Client::::new(Arc::clone(&self.handle), topic)?); - { self.primitives.clients_mtx.lock().unwrap() }.push(Arc::downgrade(&client) as Weak); + { self.primitives.clients_mtx.lock().unwrap() } + .push(Arc::downgrade(&client) as Weak); Ok(client) } diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 106fac72c..385612155 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -482,8 +482,7 @@ mod tests { .map(|value: usize| if value != 99 { 99 } else { 98 }) .unwrap_or(99); - let executor = - Context::new([], InitOptions::new().with_domain_id(Some(domain_id))) + let executor = Context::new([], InitOptions::new().with_domain_id(Some(domain_id))) .unwrap() .create_basic_executor(); let node_name = "test_publisher_names_and_types"; diff --git a/rclrs/src/node/node_options.rs b/rclrs/src/node/node_options.rs index e4172719d..ae88182cb 100644 --- a/rclrs/src/node/node_options.rs +++ b/rclrs/src/node/node_options.rs @@ -4,9 +4,9 @@ use std::{ }; use crate::{ - rcl_bindings::*, - ClockType, Node, NodeHandle, ParameterInterface, NodePrimitives, ContextHandle, - QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, + rcl_bindings::*, ClockType, ContextHandle, Node, NodeHandle, NodePrimitives, + ParameterInterface, QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, + QOS_PROFILE_CLOCK, }; /// A set of options for creating a [`Node`][1]. @@ -257,10 +257,7 @@ impl NodeOptions { /// /// Only used internally. Downstream users should call /// [`Executor::create_node`]. - pub(crate) fn build( - self, - context: &Arc, - ) -> Result { + pub(crate) fn build(self, context: &Arc) -> Result { let node_name = CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul { err, diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 11df07f28..6a30a6833 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -312,8 +312,8 @@ mod tests { }, srv::rmw::*, }, - Context, MandatoryParameter, Node, ParameterRange, ParameterValue, RclrsError, - ReadOnlyParameter, NodeOptions, Executor, SpinOptions, RclrsErrorFilter, + Context, Executor, MandatoryParameter, Node, NodeOptions, ParameterRange, ParameterValue, + RclrsError, RclrsErrorFilter, ReadOnlyParameter, SpinOptions, }; use rosidl_runtime_rs::{seq, Sequence}; use std::{ @@ -346,11 +346,9 @@ mod tests { fn construct_test_nodes(ns: &str) -> (Executor, TestNode, Node) { let executor = Context::default().create_basic_executor(); - let node = executor.create_node( - NodeOptions::new("node") - .namespace(ns) - ) - .unwrap(); + let node = executor + .create_node(NodeOptions::new("node").namespace(ns)) + .unwrap(); let range = ParameterRange { lower: Some(0), upper: Some(100), @@ -380,11 +378,9 @@ mod tests { .mandatory() .unwrap(); - let client = executor.create_node( - NodeOptions::new("client") - .namespace(ns) - ) - .unwrap(); + let client = executor + .create_node(NodeOptions::new("client").namespace(ns)) + .unwrap(); ( executor, @@ -447,12 +443,10 @@ mod tests { let inner_done = done.clone(); let rclrs_spin = tokio::task::spawn(async move { try_until_timeout(move || { - executor.spin( - SpinOptions::spin_once() - .timeout(Duration::ZERO) - ) - .timeout_ok() - .unwrap(); + executor + .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) + .timeout_ok() + .unwrap(); *inner_done.read().unwrap() }) @@ -594,12 +588,10 @@ mod tests { let rclrs_spin = tokio::task::spawn(async move { try_until_timeout(move || { println!(" -- spin"); - executor.spin( - SpinOptions::spin_once() - .timeout(Duration::ZERO) - ) - .timeout_ok() - .unwrap(); + executor + .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) + .timeout_ok() + .unwrap(); *inner_done.read().unwrap() }) @@ -657,8 +649,8 @@ mod tests { println!("checking client"); *client_finished.read().unwrap() }) - .await - .unwrap(); + .await + .unwrap(); // Set a mix of existing, non existing, dynamic and out of range parameters let bool_parameter = RmwParameter { @@ -807,8 +799,8 @@ mod tests { println!("checking client finished"); *client_finished.read().unwrap() }) - .await - .unwrap(); + .await + .unwrap(); *done.write().unwrap() = true; }); @@ -838,12 +830,10 @@ mod tests { let inner_done = done.clone(); let rclrs_spin = tokio::task::spawn(async move { try_until_timeout(move || { - executor.spin( - SpinOptions::spin_once() - .timeout(Duration::ZERO) - ) - .timeout_ok() - .unwrap(); + executor + .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) + .timeout_ok() + .unwrap(); *inner_done.read().unwrap() }) diff --git a/rclrs/src/parameter/value.rs b/rclrs/src/parameter/value.rs index 3a20ae24b..ff0c86c46 100644 --- a/rclrs/src/parameter/value.rs +++ b/rclrs/src/parameter/value.rs @@ -537,7 +537,7 @@ impl ParameterValue { #[cfg(test)] mod tests { use super::*; - use crate::{Context, RclrsError, ToResult, InitOptions}; + use crate::{Context, InitOptions, RclrsError, ToResult}; // TODO(luca) tests for all from / to ParameterVariant functions diff --git a/rclrs/src/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index ce3e82f67..8596cebd1 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,4 +1,4 @@ -use crate::{Context, Node, RclrsError, NodeOptions}; +use crate::{Context, Node, NodeOptions, RclrsError}; pub(crate) struct TestGraph { pub node1: Node, @@ -8,13 +8,7 @@ pub(crate) struct TestGraph { pub(crate) fn construct_test_graph(namespace: &str) -> Result { let executor = Context::default().create_basic_executor(); Ok(TestGraph { - node1: executor.create_node( - NodeOptions::new("graph_test_node_1") - .namespace(namespace) - )?, - node2: executor.create_node( - NodeOptions::new("graph_test_node_2") - .namespace(namespace) - )?, + node1: executor.create_node(NodeOptions::new("graph_test_node_1").namespace(namespace))?, + node2: executor.create_node(NodeOptions::new("graph_test_node_2").namespace(namespace))?, }) } diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index a5dcad50f..7cb4010c7 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -1,7 +1,7 @@ use crate::{ clock::{Clock, ClockSource, ClockType}, vendor::rosgraph_msgs::msg::Clock as ClockMsg, - Node, QoSProfile, ReadOnlyParameter, Subscription, QOS_PROFILE_CLOCK, WeakNode, + Node, QoSProfile, ReadOnlyParameter, Subscription, WeakNode, QOS_PROFILE_CLOCK, }; use std::sync::{Arc, Mutex, RwLock}; @@ -149,7 +149,10 @@ mod tests { #[test] fn time_source_default_clock() { - let node = Context::default().create_basic_executor().create_node("test_node").unwrap(); + let node = Context::default() + .create_basic_executor() + .create_node("test_node") + .unwrap(); // Default clock should be above 0 (use_sim_time is default false) assert!(node.get_clock().now().nsec > 0); } @@ -162,7 +165,7 @@ mod tests { String::from("-p"), String::from("use_sim_time:=true"), ], - InitOptions::default() + InitOptions::default(), ) .unwrap() .create_basic_executor(); From 4c2a67b0472db677ae1a840b8bb755ebcc62dc14 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Nov 2024 23:17:57 +0800 Subject: [PATCH 10/20] Fix example formatting Signed-off-by: Michael X. Grey --- .../minimal_client_service/src/minimal_client.rs | 4 +++- .../src/minimal_client_async.rs | 3 ++- .../src/minimal_service.rs | 4 +++- .../minimal_pub_sub/src/minimal_subscriber.rs | 4 +++- .../minimal_pub_sub/src/minimal_two_nodes.rs | 16 ++++++++++++---- .../minimal_pub_sub/src/zero_copy_subscriber.rs | 4 +++- examples/rust_pubsub/src/simple_subscriber.rs | 5 +---- 7 files changed, 27 insertions(+), 13 deletions(-) diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs index baeff971d..49f18e242 100644 --- a/examples/minimal_client_service/src/minimal_client.rs +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -28,5 +28,7 @@ fn main() -> Result<(), Error> { std::thread::sleep(std::time::Duration::from_millis(500)); println!("Waiting for response"); - executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) + executor + .spin(rclrs::SpinOptions::default()) + .map_err(|err| err.into()) } diff --git a/examples/minimal_client_service/src/minimal_client_async.rs b/examples/minimal_client_service/src/minimal_client_async.rs index f011569de..8babe04e7 100644 --- a/examples/minimal_client_service/src/minimal_client_async.rs +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -20,7 +20,8 @@ async fn main() -> Result<(), Error> { println!("Waiting for response"); - let rclrs_spin = tokio::task::spawn_blocking(move || executor.spin(rclrs::SpinOptions::default())); + let rclrs_spin = + tokio::task::spawn_blocking(move || executor.spin(rclrs::SpinOptions::default())); let response = future.await?; println!( diff --git a/examples/minimal_client_service/src/minimal_service.rs b/examples/minimal_client_service/src/minimal_service.rs index f38a49f21..84d154fec 100644 --- a/examples/minimal_client_service/src/minimal_service.rs +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -19,5 +19,7 @@ fn main() -> Result<(), Error> { .create_service::("add_two_ints", handle_service)?; println!("Starting server"); - executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) + executor + .spin(rclrs::SpinOptions::default()) + .map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs index 2d375bd7d..2f0820660 100644 --- a/examples/minimal_pub_sub/src/minimal_subscriber.rs +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -18,5 +18,7 @@ fn main() -> Result<(), Error> { }, )?; - executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) + executor + .spin(rclrs::SpinOptions::default()) + .map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs index f829afc52..b894b1912 100644 --- a/examples/minimal_pub_sub/src/minimal_two_nodes.rs +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -12,7 +12,11 @@ struct MinimalSubscriber { } impl MinimalSubscriber { - pub fn new(executor: &rclrs::Executor, name: &str, topic: &str) -> Result, rclrs::RclrsError> { + pub fn new( + executor: &rclrs::Executor, + name: &str, + topic: &str, + ) -> Result, rclrs::RclrsError> { let node = executor.create_node(name)?; let minimal_subscriber = Arc::new(MinimalSubscriber { num_messages: 0.into(), @@ -49,8 +53,10 @@ fn main() -> Result<(), Error> { let mut executor = rclrs::Context::default_from_env()?.create_basic_executor(); let publisher_node = executor.create_node("minimal_publisher")?; - let _subscriber_node_one = MinimalSubscriber::new(&executor, "minimal_subscriber_one", "topic")?; - let _subscriber_node_two = MinimalSubscriber::new(&executor, "minimal_subscriber_two", "topic")?; + let _subscriber_node_one = + MinimalSubscriber::new(&executor, "minimal_subscriber_one", "topic")?; + let _subscriber_node_two = + MinimalSubscriber::new(&executor, "minimal_subscriber_two", "topic")?; let publisher = publisher_node .create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; @@ -67,5 +73,7 @@ fn main() -> Result<(), Error> { } }); - executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) + executor + .spin(rclrs::SpinOptions::default()) + .map_err(|err| err.into()) } diff --git a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs index cfb569ab2..7b9f538e9 100644 --- a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -17,5 +17,7 @@ fn main() -> Result<(), Error> { }, )?; - executor.spin(rclrs::SpinOptions::default()).map_err(|err| err.into()) + executor + .spin(rclrs::SpinOptions::default()) + .map_err(|err| err.into()) } diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs index 2ba2c4e7a..4df161221 100644 --- a/examples/rust_pubsub/src/simple_subscriber.rs +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -25,10 +25,7 @@ impl SimpleSubscriptionNode { }, ) .unwrap(); - Ok(Self { - _subscriber, - data, - }) + Ok(Self { _subscriber, data }) } fn data_callback(&self) -> Result<(), RclrsError> { if let Some(data) = self.data.lock().unwrap().as_ref() { From 605950695c62eed54f02b03ea9f4f6a21f0e28a5 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 23 Nov 2024 17:03:50 +0800 Subject: [PATCH 11/20] Implicitly cast &str to NodeOptions Signed-off-by: Michael X. Grey --- rclrs/src/executor.rs | 9 +- rclrs/src/node.rs | 8 +- rclrs/src/node/node_options.rs | 298 ++++++++++++++---------- rclrs/src/parameter/service.rs | 4 +- rclrs/src/test_helpers/graph_helpers.rs | 6 +- 5 files changed, 184 insertions(+), 141 deletions(-) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 3202bd34f..64c3baee6 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -1,5 +1,5 @@ use crate::{ - rcl_bindings::rcl_context_is_valid, ContextHandle, Node, NodeOptions, RclrsError, WaitSet, + rcl_bindings::rcl_context_is_valid, ContextHandle, IntoNodeOptions, Node, RclrsError, WaitSet, WeakNode, }; use std::{ @@ -15,8 +15,11 @@ pub struct Executor { impl Executor { /// Create a [`Node`] that will run on this Executor. - pub fn create_node(&self, options: impl Into) -> Result { - let options: NodeOptions = options.into(); + pub fn create_node<'a>( + &'a self, + options: impl IntoNodeOptions<'a>, + ) -> Result { + let options = options.into_node_options(); let node = options.build(&self.context)?; self.nodes_mtx.lock().unwrap().push(node.downgrade()); Ok(node) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 42cb3a226..117a05da1 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -156,11 +156,11 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, InitOptions, RclrsError, NodeOptions}; + /// # use rclrs::{Context, InitOptions, RclrsError, IntoNodeOptions}; /// // Without remapping /// let executor = Context::default().create_basic_executor(); /// let node = executor.create_node( - /// NodeOptions::new("my_node") + /// "my_node" /// .namespace("/my/namespace") /// )?; /// assert_eq!(node.namespace(), "/my/namespace"); @@ -182,10 +182,10 @@ impl Node { /// /// # Example /// ``` - /// # use rclrs::{Context, RclrsError, NodeOptions}; + /// # use rclrs::{Context, RclrsError, IntoNodeOptions}; /// let executor = Context::default().create_basic_executor(); /// let node = executor.create_node( - /// NodeOptions::new("my_node") + /// "my_node" /// .namespace("/my/namespace") /// )?; /// assert_eq!(node.fully_qualified_name(), "/my/namespace/my_node"); diff --git a/rclrs/src/node/node_options.rs b/rclrs/src/node/node_options.rs index ae88182cb..467bbed8f 100644 --- a/rclrs/src/node/node_options.rs +++ b/rclrs/src/node/node_options.rs @@ -1,4 +1,5 @@ use std::{ + borrow::Borrow, ffi::CString, sync::{Arc, Mutex}, }; @@ -9,96 +10,13 @@ use crate::{ QOS_PROFILE_CLOCK, }; -/// A set of options for creating a [`Node`][1]. -/// -/// The builder pattern allows selectively setting some fields, and leaving all others at their default values. -/// -/// The default values for optional fields are: -/// - `namespace: "/"` -/// - `use_global_arguments: true` -/// - `arguments: []` -/// - `enable_rosout: true` -/// - `start_parameter_services: true` -/// - `clock_type: ClockType::RosTime` -/// - `clock_qos: QOS_PROFILE_CLOCK` +/// This trait helps to build [`NodeOptions`] which can be passed into +/// [`Executor::create_node`][1]. /// -/// # Example -/// ``` -/// # use rclrs::{Context, NodeOptions, Node, RclrsError}; -/// let executor = Context::default().create_basic_executor(); -/// // Building a node in a single expression -/// let node = executor.create_node(NodeOptions::new("foo_node").namespace("/bar"))?; -/// assert_eq!(node.name(), "foo_node"); -/// assert_eq!(node.namespace(), "/bar"); -/// // Building a node via NodeOptions -/// let node = executor.create_node(NodeOptions::new("bar_node"))?; -/// assert_eq!(node.name(), "bar_node"); -/// // Building a node step-by-step -/// let mut options = NodeOptions::new("goose"); -/// options = options.namespace("/duck/duck"); -/// let node = executor.create_node(options)?; -/// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose"); -/// # Ok::<(), RclrsError>(()) -/// ``` -/// -/// [1]: crate::Node -pub struct NodeOptions { - name: String, - namespace: String, - use_global_arguments: bool, - arguments: Vec, - enable_rosout: bool, - start_parameter_services: bool, - clock_type: ClockType, - clock_qos: QoSProfile, -} - -impl NodeOptions { - /// Creates a builder for a node with the given name. - /// - /// See the [`Node` docs][1] for general information on node names. - /// - /// # Rules for valid node names - /// - /// The rules for a valid node name are checked by the [`rmw_validate_node_name()`][2] - /// function. They are: - /// - Must contain only the `a-z`, `A-Z`, `0-9`, and `_` characters - /// - Must not be empty and not be longer than `RMW_NODE_NAME_MAX_NAME_LENGTH` - /// - Must not start with a number - /// - /// Note that node name validation is delayed until [`Executor::create_node`][3]. - /// - /// # Example - /// ``` - /// # use rclrs::{Context, NodeOptions, RclrsError, RclReturnCode}; - /// let executor = Context::default().create_basic_executor(); - /// // This is a valid node name - /// assert!(executor.create_node(NodeOptions::new("my_node")).is_ok()); - /// // This is another valid node name (although not a good one) - /// assert!(executor.create_node(NodeOptions::new("_______")).is_ok()); - /// // This is an invalid node name - /// assert!(matches!( - /// executor.create_node(NodeOptions::new("röböt")).unwrap_err(), - /// RclrsError::RclError { code: RclReturnCode::NodeInvalidName, .. } - /// )); - /// # Ok::<(), RclrsError>(()) - /// ``` - /// - /// [1]: crate::Node#naming - /// [2]: https://docs.ros2.org/latest/api/rmw/validate__node__name_8h.html#a5690a285aed9735f89ef11950b6e39e3 - /// [3]: crate::Executor::create_node - pub fn new(name: impl ToString) -> NodeOptions { - NodeOptions { - name: name.to_string(), - namespace: "/".to_string(), - use_global_arguments: true, - arguments: vec![], - enable_rosout: true, - start_parameter_services: true, - clock_type: ClockType::RosTime, - clock_qos: QOS_PROFILE_CLOCK, - } - } +/// [1]: crate::Executor::create_node +pub trait IntoNodeOptions<'a>: Sized { + /// Conver the object into [`NodeOptions`] with default settings. + fn into_node_options(self) -> NodeOptions<'a>; /// Sets the node namespace. /// @@ -121,15 +39,15 @@ impl NodeOptions { /// /// # Example /// ``` - /// # use rclrs::{Context, Node, NodeOptions, RclrsError, RclReturnCode}; + /// # use rclrs::{Context, Node, IntoNodeOptions, RclrsError, RclReturnCode}; /// let executor = Context::default().create_basic_executor(); /// // This is a valid namespace - /// let options_ok_ns = NodeOptions::new("my_node").namespace("/some/nested/namespace"); + /// let options_ok_ns = "my_node".namespace("/some/nested/namespace"); /// assert!(executor.create_node(options_ok_ns).is_ok()); /// // This is an invalid namespace /// assert!(matches!( /// executor.create_node( - /// NodeOptions::new("my_node") + /// "my_node" /// .namespace("/10_percent_luck/20_percent_skill") /// ).unwrap_err(), /// RclrsError::RclError { code: RclReturnCode::NodeInvalidNamespace, .. } @@ -137,7 +55,7 @@ impl NodeOptions { /// // A missing forward slash at the beginning is automatically added /// assert_eq!( /// executor.create_node( - /// NodeOptions::new("my_node") + /// "my_node" /// .namespace("foo") /// )?.namespace(), /// "/foo" @@ -149,9 +67,10 @@ impl NodeOptions { /// [2]: http://design.ros2.org/articles/topic_and_service_names.html /// [3]: https://docs.ros2.org/latest/api/rmw/validate__namespace_8h.html#a043f17d240cf13df01321b19a469ee49 /// [4]: crate::Executor::create_node - pub fn namespace(mut self, namespace: impl ToString) -> Self { - self.namespace = namespace.to_string(); - self + fn namespace(self, namespace: &'a str) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.namespace = namespace; + options } /// Enables or disables using global arguments. @@ -160,19 +79,19 @@ impl NodeOptions { /// /// # Example /// ``` - /// # use rclrs::{Context, InitOptions, Node, NodeOptions, RclrsError}; + /// # use rclrs::{Context, InitOptions, Node, IntoNodeOptions, RclrsError}; /// let context_args = ["--ros-args", "--remap", "__node:=your_node"] /// .map(String::from); /// let executor = Context::new(context_args, InitOptions::default())?.create_basic_executor(); /// // Ignore the global arguments: /// let node_without_global_args = executor.create_node( - /// NodeOptions::new("my_node") + /// "my_node" /// .use_global_arguments(false) /// )?; /// assert_eq!(node_without_global_args.name(), "my_node"); /// // Do not ignore the global arguments: /// let node_with_global_args = executor.create_node( - /// NodeOptions::new("my_other_node") + /// "my_other_node" /// .use_global_arguments(true) /// )?; /// assert_eq!(node_with_global_args.name(), "your_node"); @@ -180,9 +99,10 @@ impl NodeOptions { /// ``` /// /// [1]: crate::Context::new - pub fn use_global_arguments(mut self, enable: bool) -> Self { - self.use_global_arguments = enable; - self + fn use_global_arguments(self, enable: bool) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.use_global_arguments = enable; + options } /// Sets node-specific command line arguments. @@ -195,7 +115,7 @@ impl NodeOptions { /// /// # Example /// ``` - /// # use rclrs::{Context, InitOptions, Node, NodeOptions, RclrsError}; + /// # use rclrs::{Context, InitOptions, IntoNodeOptions, Node, RclrsError}; /// // Usually, this would change the name of "my_node" to "context_args_node": /// let context_args = ["--ros-args", "--remap", "my_node:__node:=context_args_node"] /// .map(String::from); @@ -204,7 +124,7 @@ impl NodeOptions { /// let node_args = ["--ros-args", "--remap", "my_node:__node:=node_args_node"] /// .map(String::from); /// let node = executor.create_node( - /// NodeOptions::new("my_node") + /// "my_node" /// .arguments(node_args) /// )?; /// assert_eq!(node.name(), "node_args_node"); @@ -213,12 +133,13 @@ impl NodeOptions { /// /// [1]: crate::Context::new /// [2]: https://design.ros2.org/articles/ros_command_line_arguments.html - pub fn arguments(mut self, arguments: Args) -> Self + fn arguments(self, arguments: Args) -> NodeOptions<'a> where Args::Item: ToString, { - self.arguments = arguments.into_iter().map(|item| item.to_string()).collect(); - self + let mut options = self.into_node_options(); + options.arguments = arguments.into_iter().map(|item| item.to_string()).collect(); + options } /// Enables or disables logging to rosout. @@ -227,30 +148,138 @@ impl NodeOptions { /// standard output. /// /// This option is currently unused in `rclrs`. - pub fn enable_rosout(mut self, enable: bool) -> Self { - self.enable_rosout = enable; - self + fn enable_rosout(self, enable: bool) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.enable_rosout = enable; + options } /// Enables or disables parameter services. /// /// Parameter services can be used to allow external nodes to list, get and set /// parameters for this node. - pub fn start_parameter_services(mut self, start: bool) -> Self { - self.start_parameter_services = start; - self + fn start_parameter_services(self, start: bool) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.start_parameter_services = start; + options } /// Sets the node's clock type. - pub fn clock_type(mut self, clock_type: ClockType) -> Self { - self.clock_type = clock_type; - self + fn clock_type(self, clock_type: ClockType) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.clock_type = clock_type; + options } /// Sets the QoSProfile for the clock subscription. - pub fn clock_qos(mut self, clock_qos: QoSProfile) -> Self { - self.clock_qos = clock_qos; - self + fn clock_qos(self, clock_qos: QoSProfile) -> NodeOptions<'a> { + let mut options = self.into_node_options(); + options.clock_qos = clock_qos; + options + } +} + +/// A set of options for creating a [`Node`][1]. +/// +/// The builder pattern, implemented through [`IntoNodeOptions`], allows +/// selectively setting some fields, and leaving all others at their default values. +/// +/// The default values for optional fields are: +/// - `namespace: "/"` +/// - `use_global_arguments: true` +/// - `arguments: []` +/// - `enable_rosout: true` +/// - `start_parameter_services: true` +/// - `clock_type: ClockType::RosTime` +/// - `clock_qos: QOS_PROFILE_CLOCK` +/// +/// # Example +/// ``` +/// # use rclrs::{ClockType, Context, IntoNodeOptions, NodeOptions, Node, RclrsError}; +/// let executor = Context::default().create_basic_executor(); +/// +/// // Building a node with default options +/// let node = executor.create_node("foo_node"); +/// +/// // Building a node with a namespace +/// let node = executor.create_node("bar_node".namespace("/bar"))?; +/// assert_eq!(node.name(), "bar_node"); +/// assert_eq!(node.namespace(), "/bar"); +/// +/// // Building a node with a namespace and no parameter services +/// let node = executor.create_node( +/// "baz" +/// .namespace("qux") +/// .start_parameter_services(false) +/// )?; +/// +/// // Building node options step-by-step +/// let mut options = NodeOptions::new("goose"); +/// options = options.namespace("/duck/duck"); +/// options = options.clock_type(ClockType::SteadyTime); +/// +/// let node = executor.create_node(options)?; +/// assert_eq!(node.fully_qualified_name(), "/duck/duck/goose"); +/// # Ok::<(), RclrsError>(()) +/// ``` +/// +/// [1]: crate::Node +pub struct NodeOptions<'a> { + name: &'a str, + namespace: &'a str, + use_global_arguments: bool, + arguments: Vec, + enable_rosout: bool, + start_parameter_services: bool, + clock_type: ClockType, + clock_qos: QoSProfile, +} + +impl<'a> NodeOptions<'a> { + /// Creates a builder for a node with the given name. + /// + /// See the [`Node` docs][1] for general information on node names. + /// + /// # Rules for valid node names + /// + /// The rules for a valid node name are checked by the [`rmw_validate_node_name()`][2] + /// function. They are: + /// - Must contain only the `a-z`, `A-Z`, `0-9`, and `_` characters + /// - Must not be empty and not be longer than `RMW_NODE_NAME_MAX_NAME_LENGTH` + /// - Must not start with a number + /// + /// Note that node name validation is delayed until [`Executor::create_node`][3]. + /// + /// # Example + /// ``` + /// # use rclrs::{Context, NodeOptions, RclrsError, RclReturnCode}; + /// let executor = Context::default().create_basic_executor(); + /// // This is a valid node name + /// assert!(executor.create_node(NodeOptions::new("my_node")).is_ok()); + /// // This is another valid node name (although not a good one) + /// assert!(executor.create_node(NodeOptions::new("_______")).is_ok()); + /// // This is an invalid node name + /// assert!(matches!( + /// executor.create_node(NodeOptions::new("röböt")).unwrap_err(), + /// RclrsError::RclError { code: RclReturnCode::NodeInvalidName, .. } + /// )); + /// # Ok::<(), RclrsError>(()) + /// ``` + /// + /// [1]: crate::Node#naming + /// [2]: https://docs.ros2.org/latest/api/rmw/validate__node__name_8h.html#a5690a285aed9735f89ef11950b6e39e3 + /// [3]: crate::Executor::create_node + pub fn new(name: &'a str) -> NodeOptions<'a> { + NodeOptions { + name, + namespace: "/", + use_global_arguments: true, + arguments: vec![], + enable_rosout: true, + start_parameter_services: true, + clock_type: ClockType::RosTime, + clock_qos: QOS_PROFILE_CLOCK, + } } /// Builds the node instance. @@ -258,15 +287,14 @@ impl NodeOptions { /// Only used internally. Downstream users should call /// [`Executor::create_node`]. pub(crate) fn build(self, context: &Arc) -> Result { - let node_name = - CString::new(self.name.as_str()).map_err(|err| RclrsError::StringContainsNul { - err, - s: self.name.clone(), - })?; + let node_name = CString::new(self.name).map_err(|err| RclrsError::StringContainsNul { + err, + s: self.name.to_owned(), + })?; let node_namespace = - CString::new(self.namespace.as_str()).map_err(|err| RclrsError::StringContainsNul { + CString::new(self.namespace).map_err(|err| RclrsError::StringContainsNul { err, - s: self.namespace.clone(), + s: self.namespace.to_owned(), })?; let rcl_node_options = self.create_rcl_node_options()?; let rcl_context = &mut *context.rcl_context.lock().unwrap(); @@ -367,9 +395,21 @@ impl NodeOptions { } } -impl From for NodeOptions { - fn from(name: T) -> Self { - NodeOptions::new(name) +impl<'a> IntoNodeOptions<'a> for NodeOptions<'a> { + fn into_node_options(self) -> NodeOptions<'a> { + self + } +} + +impl<'a, T: Borrow> IntoNodeOptions<'a> for &'a T { + fn into_node_options(self) -> NodeOptions<'a> { + NodeOptions::new(self.borrow()) + } +} + +impl<'a> IntoNodeOptions<'a> for &'a str { + fn into_node_options(self) -> NodeOptions<'a> { + NodeOptions::new(self) } } diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 6a30a6833..a4b65c7e0 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -312,8 +312,8 @@ mod tests { }, srv::rmw::*, }, - Context, Executor, MandatoryParameter, Node, NodeOptions, ParameterRange, ParameterValue, - RclrsError, RclrsErrorFilter, ReadOnlyParameter, SpinOptions, + Context, Executor, IntoNodeOptions, MandatoryParameter, Node, NodeOptions, ParameterRange, + ParameterValue, RclrsError, RclrsErrorFilter, ReadOnlyParameter, SpinOptions, }; use rosidl_runtime_rs::{seq, Sequence}; use std::{ diff --git a/rclrs/src/test_helpers/graph_helpers.rs b/rclrs/src/test_helpers/graph_helpers.rs index 8596cebd1..f61f5db96 100644 --- a/rclrs/src/test_helpers/graph_helpers.rs +++ b/rclrs/src/test_helpers/graph_helpers.rs @@ -1,4 +1,4 @@ -use crate::{Context, Node, NodeOptions, RclrsError}; +use crate::{Context, IntoNodeOptions, Node, RclrsError}; pub(crate) struct TestGraph { pub node1: Node, @@ -8,7 +8,7 @@ pub(crate) struct TestGraph { pub(crate) fn construct_test_graph(namespace: &str) -> Result { let executor = Context::default().create_basic_executor(); Ok(TestGraph { - node1: executor.create_node(NodeOptions::new("graph_test_node_1").namespace(namespace))?, - node2: executor.create_node(NodeOptions::new("graph_test_node_2").namespace(namespace))?, + node1: executor.create_node("graph_test_node_1".namespace(namespace))?, + node2: executor.create_node("graph_test_node_2".namespace(namespace))?, }) } From 259fcb374e05624a6cb6061bbe1d10f87bb1f7ed Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 23 Nov 2024 17:06:25 +0800 Subject: [PATCH 12/20] Remove debug outputs Signed-off-by: Michael X. Grey --- rclrs/src/parameter/service.rs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index a4b65c7e0..8fe5742ff 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -574,7 +574,6 @@ mod tests { .create_client::("/get_set/node/set_parameters_atomically")?; try_until_timeout(|| { - println!(" >> testing services"); get_client.service_is_ready().unwrap() && set_client.service_is_ready().unwrap() && set_atomically_client.service_is_ready().unwrap() @@ -587,7 +586,6 @@ mod tests { let inner_done = done.clone(); let rclrs_spin = tokio::task::spawn(async move { try_until_timeout(move || { - println!(" -- spin"); executor .spin(SpinOptions::spin_once().timeout(Duration::ZERO)) .timeout_ok() @@ -646,7 +644,6 @@ mod tests { ) .unwrap(); try_until_timeout(|| { - println!("checking client"); *client_finished.read().unwrap() }) .await @@ -796,7 +793,6 @@ mod tests { ) .unwrap(); try_until_timeout(|| { - println!("checking client finished"); *client_finished.read().unwrap() }) .await From e1ceb70cf6bc0585910d40f7eeaa34425eab5994 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 23 Nov 2024 17:11:59 +0800 Subject: [PATCH 13/20] Fix formatting Signed-off-by: Michael X. Grey --- rclrs/src/parameter/service.rs | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/rclrs/src/parameter/service.rs b/rclrs/src/parameter/service.rs index 8fe5742ff..770489e7e 100644 --- a/rclrs/src/parameter/service.rs +++ b/rclrs/src/parameter/service.rs @@ -643,11 +643,9 @@ mod tests { }, ) .unwrap(); - try_until_timeout(|| { - *client_finished.read().unwrap() - }) - .await - .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); // Set a mix of existing, non existing, dynamic and out of range parameters let bool_parameter = RmwParameter { @@ -792,11 +790,9 @@ mod tests { }, ) .unwrap(); - try_until_timeout(|| { - *client_finished.read().unwrap() - }) - .await - .unwrap(); + try_until_timeout(|| *client_finished.read().unwrap()) + .await + .unwrap(); *done.write().unwrap() = true; }); From 217f42cd30fa8229b3c46664c73e23a956c4658b Mon Sep 17 00:00:00 2001 From: geoff-imdex Date: Mon, 25 Nov 2024 04:46:24 +0800 Subject: [PATCH 14/20] Add rosout logging to rclrs (#422) * Initial logging implementation * * Moved logging_fini to follow correct call sequence based on rclcpp * Primary change to fix builder's node variable going out of scope * Fix linting issue with use statement * Remove the need to Box rcl_node_t (#5) Signed-off-by: Michael X. Grey * Fix linting issues * Fix logging lifecycles and clean up tests Signed-off-by: Michael X. Grey * Linting fixes after logging lifecycle improvements * Introduce Logger and LogParams Signed-off-by: Michael X. Grey * Cleaner logging tests Signed-off-by: Michael X. Grey Signed-off-by: Michael X. Grey Signed-off-by: Michael X. Grey Co-authored-by: Nathan Wiebe Neufeldt * Address feedback for logging PR Signed-off-by: Michael X. Grey Signed-off-by: Michael X. Grey Signed-off-by: Michael X. Grey Co-authored-by: Nathan Wiebe Neufeldt --------- Signed-off-by: Michael X. Grey Signed-off-by: Michael X. Grey Signed-off-by: Michael X. Grey Co-authored-by: Grey Co-authored-by: Nathan Wiebe Neufeldt --- rclrs/build.rs | 1 + rclrs/src/context.rs | 17 +- rclrs/src/lib.rs | 2 + rclrs/src/logging.rs | 667 +++++++++++++++++++++ rclrs/src/logging/log_params.rs | 293 +++++++++ rclrs/src/logging/logger.rs | 111 ++++ rclrs/src/logging/logging_configuration.rs | 251 ++++++++ rclrs/src/node.rs | 65 +- rclrs/src/node/builder.rs | 50 +- rclrs/src/node/graph.rs | 27 +- rclrs/src/parameter.rs | 21 +- rclrs/src/rcl_wrapper.h | 1 + rclrs/src/time_source.rs | 8 +- 13 files changed, 1479 insertions(+), 35 deletions(-) create mode 100644 rclrs/src/logging.rs create mode 100644 rclrs/src/logging/log_params.rs create mode 100644 rclrs/src/logging/logger.rs create mode 100644 rclrs/src/logging/logging_configuration.rs diff --git a/rclrs/build.rs b/rclrs/build.rs index 3ac6dc414..91d0b190f 100644 --- a/rclrs/build.rs +++ b/rclrs/build.rs @@ -35,6 +35,7 @@ fn main() { } }; + println!("cargo:rustc-check-cfg=cfg(ros_distro, values(\"humble\", \"iron\", \"jazzy\", \"rolling\"))"); println!("cargo:rustc-cfg=ros_distro=\"{ros_distro}\""); let mut builder = bindgen::Builder::default() diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 90c8fbd3c..524169bb2 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -6,7 +6,7 @@ use std::{ vec::Vec, }; -use crate::{rcl_bindings::*, RclrsError, ToResult}; +use crate::{rcl_bindings::*, LoggingLifecycle, RclrsError, ToResult}; /// This is locked whenever initializing or dropping any middleware entity /// because we have found issues in RCL and some RMW implementations that @@ -56,6 +56,10 @@ unsafe impl Send for rcl_context_t {} /// - middleware-specific data, e.g. the domain participant in DDS /// - the allocator used (left as the default by `rclrs`) /// +/// The context also configures the rcl_logging_* layer to allow publication to /rosout +/// (as well as the terminal). TODO: This behaviour should be configurable using an +/// "auto logging initialise" flag as per rclcpp and rclpy. +/// pub struct Context { pub(crate) handle: Arc, } @@ -68,6 +72,10 @@ pub struct Context { /// bindings in this library. pub(crate) struct ContextHandle { pub(crate) rcl_context: Mutex, + /// This ensures that logging does not get cleaned up until after this ContextHandle + /// has dropped. + #[allow(unused)] + logging: Arc, } impl Context { @@ -143,9 +151,16 @@ impl Context { // Move the check after the last fini() ret?; } + + // TODO: "Auto set-up logging" is forced but should be configurable as per rclcpp and rclpy + // SAFETY: We created this context a moment ago and verified that it is valid. + // No other conditions are needed. + let logging = unsafe { LoggingLifecycle::configure(&rcl_context)? }; + Ok(Self { handle: Arc::new(ContextHandle { rcl_context: Mutex::new(rcl_context), + logging, }), }) } diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 4924b36cb..3a22c6da8 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -11,6 +11,7 @@ mod clock; mod context; mod error; mod executor; +mod logging; mod node; mod parameter; mod publisher; @@ -38,6 +39,7 @@ pub use clock::*; pub use context::*; pub use error::*; pub use executor::*; +pub use logging::*; pub use node::*; pub use parameter::*; pub use publisher::*; diff --git a/rclrs/src/logging.rs b/rclrs/src/logging.rs new file mode 100644 index 000000000..5143ae35c --- /dev/null +++ b/rclrs/src/logging.rs @@ -0,0 +1,667 @@ +// Copyright (c) 2019 Sequence Planner +// SPDX-License-Identifier: Apache-2.0 AND MIT +// Adapted from https://github.com/sequenceplanner/r2r/blob/89cec03d07a1496a225751159cbc7bfb529d9dd1/r2r/src/utils.rs +// Further adapted from https://github.com/mvukov/rules_ros2/pull/371 + +use std::{ + collections::HashMap, + ffi::CString, + sync::{Mutex, OnceLock}, +}; + +use crate::{rcl_bindings::*, ENTITY_LIFECYCLE_MUTEX}; + +mod logging_configuration; +pub(crate) use logging_configuration::*; + +mod log_params; +pub use log_params::*; + +mod logger; +pub use logger::*; + +/// log a message to rosout +/// +/// # Examples +/// +/// ``` +/// use rclrs::{log, ToLogParams}; +/// use std::sync::Mutex; +/// use std::time::Duration; +/// use std::env; +/// +/// let context = rclrs::Context::new(env::args()).unwrap(); +/// let node = rclrs::Node::new(&context, "test_node").unwrap(); +/// +/// log!(node.debug(), "Simple debug message"); +/// let some_variable = 43; +/// log!(node.debug(), "Formatted debug message: {some_variable}"); +/// log!(node.fatal(), "Fatal message from {}", node.name()); +/// log!(node.warn().once(), "Only log this the first time"); +/// log!( +/// node +/// .error() +/// .skip_first() +/// .throttle(Duration::from_millis(1000)), +/// "Noisy error that we expect the first time" +/// ); +/// +/// let count = 0; +/// log!( +/// node +/// .info() +/// .throttle(Duration::from_millis(1000)) +/// .only_if(count % 10 == 0), +/// "Manually constructed LogConditions", +/// ); +/// ``` +/// +/// All of the above examples will also work with the severity-specific log macros, +/// but any severity that gets passed in will be overridden: +/// - [`log_debug`][crate::log_debug] +/// - [`log_info`][crate::log_info] +/// - [`log_warn`][crate::log_warn] +/// - [`log_error`][crate::log_error] +/// - [`log_fatal`][crate::log_fatal] +/// +/// # Panics +/// +/// It is theoretically possible for the call to panic if the Mutex used for the throttling is +/// poisoned although this should never happen. +#[macro_export] +macro_rules! log { + // The variable args captured by the $(, $($args:tt)*)?)) code allows us to omit (or include) + // formatting parameters in the simple message case, e.g. to write + // ``` + // log_error!(, "Log with no params"); // OR + // log_error!(, "Log with useful info {}", error_reason); + ($to_log_params: expr, $($args:tt)*) => {{ + // Adding these use statements here due an issue like this one: + // https://github.com/intellij-rust/intellij-rust/issues/9853 + // Note: that issue appears to be specific to jetbrains intellisense however, + // observed same/similar behaviour with rust-analyzer/rustc + use std::sync::{Once, OnceLock, Mutex}; + use std::time::{SystemTime, Instant}; + + // We wrap the functional body of the macro inside of a closure which + // we immediately trigger. This allows us to use `return` to exit the + // macro early without causing the calling function to also try to + // return early. + (|| { + let params = $crate::ToLogParams::to_log_params($to_log_params); + + if !params.get_user_filter() { + // The user filter is telling us not to log this time, so exit + // before doing anything else. + return; + } + + let mut first_time = false; + static REMEMBER_FIRST_TIME: Once = Once::new(); + REMEMBER_FIRST_TIME.call_once(|| first_time = true); + + let logger_name = params.get_logger_name(); + let severity = params.get_severity(); + + match params.get_occurence() { + // Create the static variables here so we get a per-instance static + $crate::LogOccurrence::Once => { + if first_time { + $crate::log_unconditional!(severity, logger_name, $($args)*); + } + // Since we've already logged once, we should never log again, + // so just exit right now. + return; + } + $crate::LogOccurrence::SkipFirst => { + if first_time { + // This is the first time that we're seeing this log, and we + // were told to skip the first one, so just exit right away. + return; + } + } + // Do nothing + $crate::LogOccurrence::All => (), + } + + // If we have a throttle duration then check if we're inside or outside + // of that interval. + let throttle = params.get_throttle(); + if throttle > std::time::Duration::ZERO { + match params.get_throttle_clock() { + $crate::ThrottleClock::SteadyTime => { + static LAST_LOG_STEADY_TIME: OnceLock> = OnceLock::new(); + let last_log_time = LAST_LOG_STEADY_TIME.get_or_init(|| { + Mutex::new(Instant::now()) + }); + + if !first_time { + let now = Instant::now(); + let mut previous = last_log_time.lock().unwrap(); + if now >= *previous + throttle { + *previous = now; + } else { + // We are still inside the throttle interval, so just exit here. + return; + } + } + } + $crate::ThrottleClock::SystemTime => { + static LAST_LOG_SYSTEM_TIME: OnceLock> = OnceLock::new(); + let last_log_time = LAST_LOG_SYSTEM_TIME.get_or_init(|| { + Mutex::new(SystemTime::now()) + }); + + if !first_time { + let now = SystemTime::now(); + let mut previous = last_log_time.lock().unwrap(); + if now >= *previous + throttle { + *previous = now; + } else { + // We are still inside the throttle interval, so just exit here. + return; + } + } + } + $crate::ThrottleClock::Clock(clock) => { + static LAST_LOG_CLOCK_TIME: OnceLock> = OnceLock::new(); + let last_log_time = LAST_LOG_CLOCK_TIME.get_or_init(|| { + Mutex::new(clock.now()) + }); + + if !first_time { + let now = clock.now(); + let mut previous = last_log_time.lock().unwrap(); + + let new_interval = !now.compare_with( + &(previous.clone() + throttle), + |now, interval| now < interval, + ) + .is_some_and(|eval| eval); + + if new_interval { + *previous = now; + } else { + // We are still inside the throttle interval, so just exit here. + return; + } + } + } + } + } + + // All filters have been checked, so go ahead and publish the message + $crate::log_unconditional!(severity, logger_name, $($args)*); + })(); + }}; +} + +/// Debug log message. See [`log`] for usage. +#[macro_export] +macro_rules! log_debug { + ($to_log_params: expr, $($args:tt)*) => {{ + let log_params = $crate::ToLogParams::to_log_params($to_log_params); + $crate::log!(log_params.debug(), $($args)*); + }} +} + +/// Info log message. See [`log`] for usage. +#[macro_export] +macro_rules! log_info { + ($to_log_params: expr, $($args:tt)*) => {{ + let log_params = $crate::ToLogParams::to_log_params($to_log_params); + $crate::log!(log_params.info(), $($args)*); + }} +} + +/// Warning log message. See [`log`] for usage. +#[macro_export] +macro_rules! log_warn { + ($to_log_params: expr, $($args:tt)*) => {{ + let log_params = $crate::ToLogParams::to_log_params($to_log_params); + $crate::log!(log_params.warn(), $($args)*); + }} +} + +/// Error log message. See [`log`] for usage. +#[macro_export] +macro_rules! log_error { + ($to_log_params: expr, $($args:tt)*) => {{ + let log_params = $crate::ToLogParams::to_log_params($to_log_params); + $crate::log!(log_params.error(), $($args)*); + }} +} + +/// Fatal log message. See [`log`] for usage. +#[macro_export] +macro_rules! log_fatal { + ($to_log_params: expr, $($args:tt)*) => {{ + let log_params = $crate::ToLogParams::to_log_params($to_log_params); + $crate::log!(log_params.fatal(), $($args)*); + }} +} + +/// A logging mechanism that does not have any conditions: It will definitely +/// publish the log. This is only meant for internal use, but needs to be exported +/// in order for [`log`] to work. +#[doc(hidden)] +#[macro_export] +macro_rules! log_unconditional { + ($severity: expr, $logger_name: expr, $($args:tt)*) => {{ + use std::{ffi::CString, sync::OnceLock}; + + // Only allocate a CString for the function name once per call to this macro. + static FUNCTION_NAME: OnceLock = OnceLock::new(); + let function_name = FUNCTION_NAME.get_or_init(|| { + // This call to function! is nested within two layers of closures, + // so we need to strip away those suffixes or else users will be + // misled. If we ever restructure these macros or if Rust changes + // the way it names closures, this implementation detail may need to + // change. + let function_name = $crate::function!() + .strip_suffix("::{{closure}}::{{closure}}") + .unwrap(); + + CString::new(function_name).unwrap_or( + CString::new("").unwrap() + ) + }); + + // Only allocate a CString for the file name once per call to this macro. + static FILE_NAME: OnceLock = OnceLock::new(); + let file_name = FILE_NAME.get_or_init(|| { + CString::new(file!()).unwrap_or( + CString::new("").unwrap() + ) + }); + + // We have to allocate a CString for the message every time because the + // formatted data may have changed. We could consider having an alternative + // macro for string literals that only allocates once, but it not obvious + // how to guarantee that the user only passes in an unchanging string literal. + match CString::new(std::fmt::format(format_args!($($args)*))) { + Ok(message) => { + // SAFETY: impl_log is actually completely safe to call, we just + // mark it as unsafe to discourage downstream users from calling it + unsafe { $crate::impl_log($severity, $logger_name, &message, &function_name, &file_name, line!()) }; + } + Err(err) => { + let message = CString::new(format!( + "Unable to format log message into a valid c-string. Error: {}", err + )).unwrap(); + + // SAFETY: impl_log is actually completely safe to call, we just + // mark it as unsafe to discourage downstream users from calling it + unsafe { + $crate::impl_log( + $crate::LogSeverity::Error, + &$crate::LoggerName::Unvalidated("logger"), + &message, + &function_name, + &file_name, + line!(), + ); + } + } + } + }} +} + +/// Calls the underlying rclutils logging function +/// Don't call this directly, use the logging macros instead, i.e. [`log`]. +/// +/// SAFETY: This function is not actually unsafe, but it is not meant to be part of the public +/// API, so we mark it as unsafe to discourage users from trying to use it. They should use +/// one of the of log! macros instead. We can't make it private because it needs to be used +/// by exported macros. +#[doc(hidden)] +pub unsafe fn impl_log( + severity: LogSeverity, + logger_name: &LoggerName, + message: &CString, + function: &CString, + file: &CString, + line: u32, +) { + // We use a closure here because there are several different points in this + // function where we may need to run this same logic. + let send_log = |severity: LogSeverity, logger_name: &CString, message: &CString| { + let location = rcutils_log_location_t { + function_name: function.as_ptr(), + file_name: file.as_ptr(), + line_number: line as usize, + }; + + static FORMAT_STRING: OnceLock = OnceLock::new(); + let format_string = FORMAT_STRING.get_or_init(|| CString::new("%s").unwrap()); + + let severity = severity.as_native(); + + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + + #[cfg(test)] + { + // If we are compiling for testing purposes, when the default log + // output handler is being used we need to use the format_string, + // but when our custom log output handler is being used we need to + // pass the raw message string so that it can be viewed by the + // custom log output handler, allowing us to use it for test assertions. + if log_handler::is_using_custom_handler() { + // We are using the custom log handler that is only used during + // logging tests, so pass the raw message as the format string. + unsafe { + // SAFETY: The global mutex is locked as _lifecycle + rcutils_log( + &location, + severity as i32, + logger_name.as_ptr(), + message.as_ptr(), + ); + } + } else { + // We are using the normal log handler so call rcutils_log the normal way. + unsafe { + // SAFETY: The global mutex is locked as _lifecycle + rcutils_log( + &location, + severity as i32, + logger_name.as_ptr(), + format_string.as_ptr(), + message.as_ptr(), + ); + } + } + } + + #[cfg(not(test))] + { + unsafe { + // SAFETY: The global mutex is locked as _lifecycle + rcutils_log( + &location, + severity as i32, + logger_name.as_ptr(), + format_string.as_ptr(), + message.as_ptr(), + ); + } + } + }; + + match logger_name { + LoggerName::Validated(c_name) => { + // The logger name is pre-validated, so just go ahead and use it. + send_log(severity, c_name, message); + } + LoggerName::Unvalidated(str_name) => { + // The name was not validated before being passed in. + // + // We maintain a hashmap of previously validated loggers so + // we don't need to reallocate the CString on every log instance. + // This is done inside of the function impl_log instead of in a macro + // so that this map is global for the entire application. + static NAME_MAP: OnceLock>> = OnceLock::new(); + let name_map = NAME_MAP.get_or_init(Default::default); + + { + // We need to keep name_map locked while we call send_log, but + // we also need to make sure it gets unlocked right afterward + // if the condition fails, otherwise this function would + // deadlock on itself when handling the error case of the logger + // name being invalid. So we keep name_map_guard in this extra + // scope to isolate its lifespan. + let name_map_guard = name_map.lock().unwrap(); + if let Some(c_name) = name_map_guard.get(*str_name) { + // The map name has been used before, so we just use the + // pre-existing CString + send_log(severity, c_name, message); + + // We return right away because the remainder of this + // function just allocates and validates a new CString for + // the logger name. + return; + } + } + + // The unvalidated logger name has not been used before, so we need + // to convert it and add it to the name_map now. + let c_name = match CString::new(str_name.to_string()) { + Ok(c_name) => c_name, + Err(_) => { + static INVALID_MSG: OnceLock = OnceLock::new(); + let invalid_msg = INVALID_MSG.get_or_init(|| { + CString::new( + "Failed to convert logger name into a c-string. \ + Check for null terminators inside the string.", + ) + .unwrap() + }); + static INTERNAL_LOGGER_NAME: OnceLock = OnceLock::new(); + let internal_logger_name = + INTERNAL_LOGGER_NAME.get_or_init(|| CString::new("logger").unwrap()); + send_log(severity, internal_logger_name, invalid_msg); + return; + } + }; + + send_log(severity, &c_name, message); + name_map + .lock() + .unwrap() + .insert(str_name.to_string(), c_name); + } + } +} + +/// Used internally by logging macros to get the name of the function that called the +/// logging macro. This is not meant for public use, but we need to export it so the +/// other exported macros can use it. We should remove it if an official function! macro +/// is ever offered. +#[macro_export] +#[doc(hidden)] +macro_rules! function { + () => {{ + fn f() {} + fn type_name_of(_: T) -> &'static str { + std::any::type_name::() + } + let name = type_name_of(f); + name.strip_suffix("::f").unwrap() + }}; +} + +#[cfg(test)] +mod tests { + use crate::{log_handler::*, test_helpers::*, *}; + use std::sync::Mutex; + + #[test] + fn test_logging_macros() -> Result<(), RclrsError> { + // This test ensures that strings which are being sent to the logger are + // being sanitized correctly. Rust generally and our logging macro in + // particular do not use C-style formatting strings, but rcutils expects + // to receive C-style formatting strings alongside variadic arguments + // that describe how to fill in the formatting. + // + // If we pass the final string into rcutils as the format with no + // variadic arguments, then it may trigger a crash or undefined behavior + // if the message happens to contain any % symbols. In particular %n + // will trigger a crash when no variadic arguments are given because it + // attempts to write to a buffer. If no buffer is given, a seg fault + // happens. + log!("please do not crash", "%n"); + + let graph = construct_test_graph("test_logging_macros")?; + + let log_collection: Arc>>> = Arc::new(Mutex::new(Vec::new())); + let inner_log_collection = log_collection.clone(); + + log_handler::set_logging_output_handler(move |log_entry: log_handler::LogEntry| { + inner_log_collection + .lock() + .unwrap() + .push(log_entry.into_owned()); + }) + .unwrap(); + + let last_logger_name = || { + log_collection + .lock() + .unwrap() + .last() + .unwrap() + .logger_name + .clone() + }; + + let last_message = || { + log_collection + .lock() + .unwrap() + .last() + .unwrap() + .message + .clone() + }; + + let last_location = || { + log_collection + .lock() + .unwrap() + .last() + .unwrap() + .location + .clone() + }; + + let last_severity = || log_collection.lock().unwrap().last().unwrap().severity; + + let count_message = |message: &str| { + let mut count = 0; + for log in log_collection.lock().unwrap().iter() { + if log.message == message { + count += 1; + } + } + count + }; + + let node = graph.node1; + + log!(&*node, "Logging with node dereference"); + assert_eq!(last_logger_name(), node.logger().name()); + assert_eq!(last_message(), "Logging with node dereference"); + assert_eq!(last_severity(), LogSeverity::Info); + assert_eq!( + last_location().function_name, + "rclrs::logging::tests::test_logging_macros", + ); + + for _ in 0..10 { + log!(node.once(), "Logging once"); + } + assert_eq!(count_message("Logging once"), 1); + assert_eq!(last_severity(), LogSeverity::Info); + + log!(node.logger(), "Logging with node logger"); + assert_eq!(last_message(), "Logging with node logger"); + assert_eq!(last_severity(), LogSeverity::Info); + + log!(node.debug(), "Debug from node"); + // The default severity level is Info so we should not see the last message + assert_ne!(last_message(), "Debug from node"); + assert_ne!(last_severity(), LogSeverity::Debug); + + log!(node.info(), "Info from node"); + assert_eq!(last_message(), "Info from node"); + assert_eq!(last_severity(), LogSeverity::Info); + + log!(node.warn(), "Warn from node"); + assert_eq!(last_message(), "Warn from node"); + assert_eq!(last_severity(), LogSeverity::Warn); + + log!(node.error(), "Error from node"); + assert_eq!(last_message(), "Error from node"); + assert_eq!(last_severity(), LogSeverity::Error); + + log!(node.fatal(), "Fatal from node"); + assert_eq!(last_message(), "Fatal from node"); + assert_eq!(last_severity(), LogSeverity::Fatal); + + log_debug!(node.logger(), "log_debug macro"); + log_info!(node.logger(), "log_info macro"); + log_warn!(node.logger(), "log_warn macro"); + log_error!(node.logger(), "log_error macro"); + log_fatal!(node.logger(), "log_fatal macro"); + + log!(node.only_if(false), "This should not be logged",); + log!(node.only_if(true), "This should be logged",); + + for i in 0..3 { + log!(node.warn().skip_first(), "Formatted warning #{}", i); + } + assert_eq!(count_message("Formatted warning #0"), 0); + assert_eq!(count_message("Formatted warning #1"), 1); + assert_eq!(count_message("Formatted warning #2"), 1); + + node.logger().set_level(LogSeverity::Debug).unwrap(); + log_debug!(node.logger(), "This debug message appears"); + assert_eq!(last_message(), "This debug message appears"); + assert_eq!(last_severity(), LogSeverity::Debug); + + node.logger().set_level(LogSeverity::Info).unwrap(); + log_debug!(node.logger(), "This debug message does not appear"); + assert_ne!(last_message(), "This debug message does not appear"); + + log!("custom logger name", "message for custom logger"); + assert_eq!(last_logger_name(), "custom logger name"); + assert_eq!(last_message(), "message for custom logger"); + + for _ in 0..3 { + log!( + "custom logger name once".once(), + "one-time message for custom logger", + ); + } + assert_eq!(last_logger_name(), "custom logger name once"); + assert_eq!(last_severity(), LogSeverity::Info); + assert_eq!(count_message("one-time message for custom logger"), 1); + + for _ in 0..3 { + log!( + "custom logger name skip".error().skip_first(), + "error for custom logger", + ); + } + assert_eq!(last_logger_name(), "custom logger name skip"); + assert_eq!(last_severity(), LogSeverity::Error); + assert_eq!(count_message("error for custom logger"), 2); + + // Test whether throttling works correctly with a ROS clock + let (clock, source) = Clock::with_source(); + source.set_ros_time_override(0); + + for i in 0..15 { + log!( + "logger" + .throttle(Duration::from_nanos(10)) + .throttle_clock(ThrottleClock::Clock(&clock)), + "custom clock throttled message", + ); + source.set_ros_time_override(i); + } + + // The throttle interval is 10ns and the loop shifted the time from 0ns + // to 14ns, triggering the log macro once per nanosecond. That means we + // should see two messages in the log. + assert_eq!(count_message("custom clock throttled message"), 2); + + reset_logging_output_handler(); + Ok(()) + } + + #[test] + fn test_function_macro() { + assert_eq!(function!(), "rclrs::logging::tests::test_function_macro"); + } +} diff --git a/rclrs/src/logging/log_params.rs b/rclrs/src/logging/log_params.rs new file mode 100644 index 000000000..4b67edc50 --- /dev/null +++ b/rclrs/src/logging/log_params.rs @@ -0,0 +1,293 @@ +use crate::{rcl_bindings::RCUTILS_LOG_SEVERITY, Clock}; +use std::{borrow::Borrow, ffi::CString, time::Duration}; + +/// These parameters determine the behavior of an instance of logging. +#[derive(Debug, Clone, Copy)] +pub struct LogParams<'a> { + /// The name of the logger + logger_name: LoggerName<'a>, + /// The severity of the logging instance. + severity: LogSeverity, + /// Specify when a log message should be published (See[`LoggingOccurrence`] above) + occurs: LogOccurrence, + /// Specify a publication throttling interval for the message. A value of ZERO (0) indicates that the + /// message should not be throttled. Otherwise, the message will only be published once the specified + /// interval has elapsed. This field is typically used to limit the output from high-frequency messages, + /// e.g. if `log!(logger.throttle(Duration::from_secs(1)), "message");` is called every 10ms, it will + /// nevertheless only be published once per second. + throttle: Duration, + /// Specify a clock to use for throttling. By default this will be [`ThrottleClock::SteadyTime`]. + throttle_clock: ThrottleClock<'a>, + /// The log message will only published if the specified expression evaluates to true + only_if: bool, +} + +impl<'a> LogParams<'a> { + /// Create a set of default log parameters, given the name of a logger + pub fn new(logger_name: LoggerName<'a>) -> Self { + Self { + logger_name, + severity: Default::default(), + occurs: Default::default(), + throttle: Duration::new(0, 0), + throttle_clock: Default::default(), + only_if: true, + } + } + + /// Get the logger name + pub fn get_logger_name(&self) -> &LoggerName { + &self.logger_name + } + + /// Get the severity of the log + pub fn get_severity(&self) -> LogSeverity { + self.severity + } + + /// Get the occurrence + pub fn get_occurence(&self) -> LogOccurrence { + self.occurs + } + + /// Get the throttle interval duration + pub fn get_throttle(&self) -> Duration { + self.throttle + } + + /// Get the throttle clock + pub fn get_throttle_clock(&self) -> ThrottleClock<'a> { + self.throttle_clock + } + + /// Get the arbitrary filter set by the user + pub fn get_user_filter(&self) -> bool { + self.only_if + } +} + +/// Methods for defining the behavior of a logging instance. +/// +/// This trait is implemented by Logger, Node, and anything that a `&str` can be +/// [borrowed][1] from, such as string literals (`"my_str"`), [`String`], or +/// [`Cow`][2]. +/// +/// [1]: Borrow +/// [2]: std::borrow::Cow +pub trait ToLogParams<'a>: Sized { + /// Convert the object into LogParams with default settings + fn to_log_params(self) -> LogParams<'a>; + + /// The logging should only happen once + fn once(self) -> LogParams<'a> { + self.occurs(LogOccurrence::Once) + } + + /// The first time the logging happens, we should skip it + fn skip_first(self) -> LogParams<'a> { + self.occurs(LogOccurrence::SkipFirst) + } + + /// Set the occurrence behavior of the log instance + fn occurs(self, occurs: LogOccurrence) -> LogParams<'a> { + let mut params = self.to_log_params(); + params.occurs = occurs; + params + } + + /// Set a throttling interval during which this log will not publish. A value + /// of zero will never block the message from being published, and this is the + /// default behavior. + /// + /// A negative duration is not valid, but will be treated as a zero duration. + fn throttle(self, throttle: Duration) -> LogParams<'a> { + let mut params = self.to_log_params(); + params.throttle = throttle; + params + } + + /// Set the clock that will be used to control [throttling][Self::throttle]. + fn throttle_clock(self, clock: ThrottleClock<'a>) -> LogParams<'a> { + let mut params = self.to_log_params(); + params.throttle_clock = clock; + params + } + + /// The log will not be published if a `false` expression is passed into + /// this function. + /// + /// Other factors may prevent the log from being published if a `true` is + /// passed in, such as `ToLogParams::throttle` or `ToLogParams::once` + /// filtering the log. + fn only_if(self, only_if: bool) -> LogParams<'a> { + let mut params = self.to_log_params(); + params.only_if = only_if; + params + } + + /// Log as a debug message. + fn debug(self) -> LogParams<'a> { + self.severity(LogSeverity::Debug) + } + + /// Log as an informative message. This is the default, so you don't + /// generally need to use this. + fn info(self) -> LogParams<'a> { + self.severity(LogSeverity::Info) + } + + /// Log as a warning message. + fn warn(self) -> LogParams<'a> { + self.severity(LogSeverity::Warn) + } + + /// Log as an error message. + fn error(self) -> LogParams<'a> { + self.severity(LogSeverity::Error) + } + + /// Log as a fatal message. + fn fatal(self) -> LogParams<'a> { + self.severity(LogSeverity::Fatal) + } + + /// Set the severity for this instance of logging. The default value will be + /// [`LogSeverity::Info`]. + fn severity(self, severity: LogSeverity) -> LogParams<'a> { + let mut params = self.to_log_params(); + params.severity = severity; + params + } +} + +/// This is used to borrow a logger name which might be validated (e.g. came +/// from a [`Logger`][1] struct) or not (e.g. a user-defined `&str`). If an +/// unvalidated logger name is used with one of the logging macros, we will log +/// an error about it, and the original log message will be logged with the +/// default logger. +/// +/// [1]: crate::Logger +#[derive(Debug, Clone, Copy)] +pub enum LoggerName<'a> { + /// The logger name is already available as a valid CString + Validated(&'a CString), + /// The logger name has not been validated yet + Unvalidated(&'a str), +} + +/// Logging severity. +// +// TODO(@mxgrey): Consider whether this is redundant with RCUTILS_LOG_SEVERITY. +// Perhaps we can customize the output of bindgen to automatically change the name +// of RCUTILS_LOG_SEVERITY to just LogSeverity so it's more idiomatic and then +// export it from the rclrs module. +#[doc(hidden)] +#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)] +pub enum LogSeverity { + /// Use the severity level of the parent logger (or the root logger if the + /// current logger has no parent) + Unset, + /// For messages that are not needed outside of debugging. + Debug, + /// For messages that provide useful information about the state of the + /// application. + Info, + /// For messages that indicate something unusual or unintended might have happened. + Warn, + /// For messages that indicate an error has occurred which may cause the application + /// to misbehave. + Error, + /// For messages that indicate an error has occurred which is so severe that the + /// application should terminate because it cannot recover. + /// + /// Using this severity level will not automatically cause the application to + /// terminate, the application developer must decide how to do that on a + /// case-by-case basis. + Fatal, +} + +impl LogSeverity { + pub(super) fn as_native(&self) -> RCUTILS_LOG_SEVERITY { + use crate::rcl_bindings::rcl_log_severity_t::*; + match self { + LogSeverity::Unset => RCUTILS_LOG_SEVERITY_UNSET, + LogSeverity::Debug => RCUTILS_LOG_SEVERITY_DEBUG, + LogSeverity::Info => RCUTILS_LOG_SEVERITY_INFO, + LogSeverity::Warn => RCUTILS_LOG_SEVERITY_WARN, + LogSeverity::Error => RCUTILS_LOG_SEVERITY_ERROR, + LogSeverity::Fatal => RCUTILS_LOG_SEVERITY_FATAL, + } + } + + /// This is only used by the log output handler during testing, so it will + /// not be compiled when testing is not configured + #[cfg(test)] + pub(crate) fn from_native(native: i32) -> Self { + use crate::rcl_bindings::rcl_log_severity_t::*; + match native { + _ if native == RCUTILS_LOG_SEVERITY_UNSET as i32 => LogSeverity::Unset, + _ if native == RCUTILS_LOG_SEVERITY_DEBUG as i32 => LogSeverity::Debug, + _ if native == RCUTILS_LOG_SEVERITY_INFO as i32 => LogSeverity::Info, + _ if native == RCUTILS_LOG_SEVERITY_WARN as i32 => LogSeverity::Warn, + _ if native == RCUTILS_LOG_SEVERITY_ERROR as i32 => LogSeverity::Error, + _ if native == RCUTILS_LOG_SEVERITY_FATAL as i32 => LogSeverity::Fatal, + _ => panic!("Invalid native severity received: {}", native), + } + } +} + +impl Default for LogSeverity { + fn default() -> Self { + Self::Info + } +} + +/// Specify when a log message should be published +#[derive(Debug, Clone, Copy)] +pub enum LogOccurrence { + /// Every message will be published if all other conditions are met + All, + /// The message will only be published on the first occurrence (Note: no other conditions apply) + Once, + /// The log message will not be published on the first occurrence, but will be published on + /// each subsequent occurrence (assuming all other conditions are met) + SkipFirst, +} + +/// This parameter can specify a type of clock for a logger to use when throttling. +#[derive(Debug, Default, Clone, Copy)] +pub enum ThrottleClock<'a> { + /// Use [`std::time::Instant`] as a clock. + #[default] + SteadyTime, + /// Use [`std::time::SystemTime`] as a clock. + SystemTime, + /// Use some [`Clock`] as a clock. + Clock(&'a Clock), +} + +impl Default for LogOccurrence { + fn default() -> Self { + Self::All + } +} + +// Anything that we can borrow a string from can be used as if it's a logger and +// turned into LogParams +impl<'a, T: Borrow> ToLogParams<'a> for &'a T { + fn to_log_params(self) -> LogParams<'a> { + LogParams::new(LoggerName::Unvalidated(self.borrow())) + } +} + +impl<'a> ToLogParams<'a> for &'a str { + fn to_log_params(self) -> LogParams<'a> { + LogParams::new(LoggerName::Unvalidated(self)) + } +} + +impl<'a> ToLogParams<'a> for LogParams<'a> { + fn to_log_params(self) -> LogParams<'a> { + self + } +} diff --git a/rclrs/src/logging/logger.rs b/rclrs/src/logging/logger.rs new file mode 100644 index 000000000..30770919c --- /dev/null +++ b/rclrs/src/logging/logger.rs @@ -0,0 +1,111 @@ +use std::{borrow::Borrow, ffi::CString, sync::Arc}; + +use crate::{ + rcl_bindings::{rcutils_logging_set_default_logger_level, rcutils_logging_set_logger_level}, + LogParams, LogSeverity, LoggerName, RclrsError, ToLogParams, ToResult, ENTITY_LIFECYCLE_MUTEX, +}; + +/// Logger can be passed in as the first argument into one of the [logging][1] +/// macros provided by rclrs. When passing it into one of the logging macros, +/// you can optionally apply any of the methods from [`ToLogParams`] to tweak +/// the logging behavior. +/// +/// You can obtain a Logger in the following ways: +/// - Calling [`Node::logger`][2] to get the logger of a Node +/// - Using [`Logger::create_child`] to create a child logger for an existing logger +/// - Using [`Logger::new`] to create a new logger with any name that you'd like +/// - Using [`Logger::default()`] to access the global default logger +/// +/// Note that if you are passing the Logger of a Node into one of the logging macros, +/// then you can choose to simply pass in `&node` instead of `node.logger()`. +/// +/// [1]: crate::log +/// [2]: crate::Node::logger +#[derive(Debug, Clone, PartialEq, Eq, PartialOrd, Ord)] +pub struct Logger { + name: Arc, + c_name: Arc, +} + +impl Logger { + /// Create a new logger with the given name. + pub fn new(name: impl Borrow) -> Result { + let name: Arc = name.borrow().into(); + let c_name = match CString::new((*name).to_owned()) { + Ok(c_name) => c_name, + Err(err) => { + return Err(RclrsError::StringContainsNul { + s: (*name).to_owned(), + err, + }); + } + }; + + Ok(Self { + name, + c_name: Arc::new(c_name), + }) + } + + /// Create a new logger which will be a child of this logger. + /// + /// If the name of this logger is `parent_name`, then the name for the new + /// child will be '"parent_name.child_name"`. + /// + /// If this is the default logger (whose name is an empty string), then the + /// name for the new child will simply be the value in `child_name`. + pub fn create_child(&self, child_name: impl Borrow) -> Result { + if self.name.is_empty() { + Self::new(child_name) + } else { + Self::new(format!("{}.{}", &self.name, child_name.borrow())) + } + } + + /// Get the name of this logger + pub fn name(&self) -> &str { + &self.name + } + + /// Set the severity level of this logger + pub fn set_level(&self, severity: LogSeverity) -> Result<(), RclrsError> { + // SAFETY: The precondition are: + // - we are passing in a valid CString, which is already taken care of during construction of the Logger + // - the severity level is a valid value, which is guaranteed by the rigid enum definition + // - not thread-safe, so we lock the global mutex before calling this + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + rcutils_logging_set_logger_level(self.c_name.as_ptr(), severity.as_native() as i32).ok() + } + } + + /// Set the severity level of the default logger which acts as the root ancestor + /// of all other loggers. + pub fn set_default_level(severity: LogSeverity) { + // SAFETY: The preconditions are: + // - the severity level is a valid value, which is guaranteed by the rigid enum definition + // - not thread-safe, so we lock the global mutex before calling this + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + rcutils_logging_set_default_logger_level(severity.as_native() as i32); + } + } +} + +impl<'a> ToLogParams<'a> for &'a Logger { + fn to_log_params(self) -> LogParams<'a> { + LogParams::new(LoggerName::Validated(&self.c_name)) + } +} + +impl Default for Logger { + fn default() -> Self { + Self::new("").unwrap() + } +} + +impl std::hash::Hash for Logger { + fn hash(&self, state: &mut H) { + self.name.hash(state); + } +} diff --git a/rclrs/src/logging/logging_configuration.rs b/rclrs/src/logging/logging_configuration.rs new file mode 100644 index 000000000..1012608ec --- /dev/null +++ b/rclrs/src/logging/logging_configuration.rs @@ -0,0 +1,251 @@ +use std::sync::{Arc, Mutex, OnceLock, Weak}; + +use crate::{rcl_bindings::*, RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX}; + +struct LoggingConfiguration { + lifecycle: Mutex>, +} + +pub(crate) struct LoggingLifecycle; + +impl LoggingLifecycle { + fn new(args: &rcl_arguments_t) -> Result { + // SAFETY: + // * Lock the mutex as we cannot guarantee that rcl_* functions are protecting their global variables + // * This is only called by Self::configure, which requires that a valid context was passed to it + // * No other preconditions for calling this function + unsafe { + let allocator = rcutils_get_default_allocator(); + let _lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + rcl_logging_configure(args, &allocator).ok()?; + } + Ok(Self) + } + + /// SAFETY: Ensure rcl_context_t is valid before passing it in. + pub(crate) unsafe fn configure( + context: &rcl_context_t, + ) -> Result, RclrsError> { + static CONFIGURATION: OnceLock = OnceLock::new(); + let configuration = CONFIGURATION.get_or_init(|| LoggingConfiguration { + lifecycle: Mutex::new(Weak::new()), + }); + + let mut lifecycle = configuration.lifecycle.lock().unwrap(); + if let Some(arc_lifecycle) = lifecycle.upgrade() { + return Ok(arc_lifecycle); + } + let arc_lifecycle = Arc::new(LoggingLifecycle::new(&context.global_arguments)?); + *lifecycle = Arc::downgrade(&arc_lifecycle); + Ok(arc_lifecycle) + } +} + +impl Drop for LoggingLifecycle { + fn drop(&mut self) { + let _lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + rcl_logging_fini(); + } + } +} + +#[cfg(test)] +pub(crate) mod log_handler { + //! This module provides a way to customize how log output is handled. For + //! now we are not making this a private API and are only using it for tests + //! in rclrs. We can consider making it public in the future, but we should + //! put more consideration into the API before doing so, and more crucially + //! we need to figure out a way to process C-style formatting strings with + //! a [`va_list`] from inside of Rust, which seems to be very messy. + + use std::{ + borrow::Cow, + ffi::CStr, + sync::{ + atomic::{AtomicBool, Ordering}, + OnceLock, + }, + }; + + use crate::{rcl_bindings::*, LogSeverity, ENTITY_LIFECYCLE_MUTEX}; + + /// Global variable that allows a custom log handler to be set. This log + /// handler will be applied throughout the entire application and cannot be + /// replaced with a different custom log handler. If you want to be able to + /// change the log handler over the lifetime of your application, you should + /// design your own custom handler with an Arc> inside that allows + /// its own behavior to be modified. + static LOGGING_OUTPUT_HANDLER: OnceLock = OnceLock::new(); + + /// Alias for an arbitrary log handler that is compatible with raw rcl types + pub(crate) type RawLogHandler = Box< + dyn Fn( + *const rcutils_log_location_t, // location + std::os::raw::c_int, // severity + *const std::os::raw::c_char, // logger name + rcutils_time_point_value_t, // timestamp + *const std::os::raw::c_char, // format + *mut va_list, // formatting arguments + ) + + 'static + + Send + + Sync, + >; + + /// This is an idiomatic representation of all the information for a log entry + #[derive(Clone)] + pub(crate) struct LogEntry<'a> { + pub(crate) location: LogLocation<'a>, + pub(crate) severity: LogSeverity, + pub(crate) logger_name: Cow<'a, str>, + pub(crate) timestamp: i64, + pub(crate) message: Cow<'a, str>, + } + + impl<'a> LogEntry<'a> { + /// Change the entry from something borrowed into something owned + pub(crate) fn into_owned(self) -> LogEntry<'static> { + LogEntry { + location: self.location.into_owned(), + severity: self.severity, + logger_name: Cow::Owned(self.logger_name.into_owned()), + timestamp: self.timestamp, + message: Cow::Owned(self.message.into_owned()), + } + } + } + + /// Rust-idiomatic representation of the location of a log + #[derive(Debug, Clone)] + pub(crate) struct LogLocation<'a> { + pub function_name: Cow<'a, str>, + pub file_name: Cow<'a, str>, + pub line_number: usize, + } + + impl<'a> LogLocation<'a> { + pub(crate) fn into_owned(self) -> LogLocation<'static> { + LogLocation { + function_name: Cow::Owned(self.function_name.into_owned()), + file_name: Cow::Owned(self.file_name.into_owned()), + line_number: self.line_number, + } + } + } + + #[derive(Debug)] + pub(crate) struct OutputHandlerAlreadySet; + + static USING_CUSTOM_HANDLER: OnceLock = OnceLock::new(); + + /// Set an idiomatic log hander + pub(crate) fn set_logging_output_handler( + handler: impl Fn(LogEntry) + 'static + Send + Sync, + ) -> Result<(), OutputHandlerAlreadySet> { + let raw_handler = Box::new( + move |raw_location: *const rcutils_log_location_t, + raw_severity: std::os::raw::c_int, + raw_logger_name: *const std::os::raw::c_char, + raw_timestamp: rcutils_time_point_value_t, + raw_format: *const std::os::raw::c_char, + // NOTE: In the rclrs logging test we are choosing to format + // the full message in advance when using the custom handler, + // so the format field always contains the finished formatted + // message. Therefore we can just ignore the raw formatting + // arguments. + _raw_formatting_arguments: *mut va_list| { + unsafe { + // NOTE: We use .unwrap() extensively inside this function because + // it only gets used during tests. We should reconsider this if + // we ever make this public. + let location = LogLocation { + function_name: Cow::Borrowed( + CStr::from_ptr((*raw_location).function_name) + .to_str() + .unwrap(), + ), + file_name: Cow::Borrowed( + CStr::from_ptr((*raw_location).file_name).to_str().unwrap(), + ), + line_number: (*raw_location).line_number, + }; + let severity = LogSeverity::from_native(raw_severity); + let logger_name = + Cow::Borrowed(CStr::from_ptr(raw_logger_name).to_str().unwrap()); + let timestamp: i64 = raw_timestamp; + let message = Cow::Borrowed(CStr::from_ptr(raw_format).to_str().unwrap()); + handler(LogEntry { + location, + severity, + logger_name, + timestamp, + message, + }); + } + }, + ); + + set_raw_logging_output_handler(raw_handler) + } + + /// Set the logging output handler directly + pub(crate) fn set_raw_logging_output_handler( + handler: RawLogHandler, + ) -> Result<(), OutputHandlerAlreadySet> { + LOGGING_OUTPUT_HANDLER + .set(handler) + .map_err(|_| OutputHandlerAlreadySet)?; + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + // SAFETY: + // - We have locked the global mutex + rcutils_logging_set_output_handler(Some(rclrs_logging_output_handler)); + } + + USING_CUSTOM_HANDLER + .get_or_init(|| AtomicBool::new(false)) + .store(true, Ordering::Release); + Ok(()) + } + + pub(crate) fn is_using_custom_handler() -> bool { + USING_CUSTOM_HANDLER + .get_or_init(|| AtomicBool::new(false)) + .load(Ordering::Acquire) + } + + /// This function exists so that we can give a raw function pointer to + /// rcutils_logging_set_output_handler, which is needed by its API. + extern "C" fn rclrs_logging_output_handler( + location: *const rcutils_log_location_t, + severity: std::os::raw::c_int, + logger_name: *const std::os::raw::c_char, + timestamp: rcutils_time_point_value_t, + message: *const std::os::raw::c_char, + logging_output: *mut va_list, + ) { + let handler = LOGGING_OUTPUT_HANDLER.get().unwrap(); + (*handler)( + location, + severity, + logger_name, + timestamp, + message, + logging_output, + ); + } + + /// Reset the logging output handler to the default one + pub(crate) fn reset_logging_output_handler() { + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + // SAFETY: The global mutex is locked. No other precondition is + // required. + rcutils_logging_set_output_handler(Some(rcl_logging_multiple_output_handler)); + } + USING_CUSTOM_HANDLER + .get_or_init(|| AtomicBool::new(false)) + .store(false, Ordering::Release); + } +} diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 97684d6bc..b51b59817 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -5,7 +5,7 @@ use std::{ ffi::CStr, fmt, os::raw::c_char, - sync::{Arc, Mutex, Weak}, + sync::{atomic::AtomicBool, Arc, Mutex, Weak}, vec::Vec, }; @@ -13,10 +13,10 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - TimeSource, ENTITY_LIFECYCLE_MUTEX, + rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, LogParams, + Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, + QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, + SubscriptionCallback, TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -44,7 +44,7 @@ unsafe impl Send for rcl_node_t {} /// It's a good idea for node names in the same executable to be unique. /// /// ## Remapping -/// The namespace and name given when creating the node can be overriden through the command line. +/// The namespace and name given when creating the node can be overridden through the command line. /// In that sense, the parameters to the node creation functions are only the _default_ namespace and /// name. /// See also the [official tutorial][1] on the command line arguments for ROS nodes, and the @@ -66,23 +66,48 @@ pub struct Node { time_source: TimeSource, parameter: ParameterInterface, pub(crate) handle: Arc, + logger: Logger, } /// This struct manages the lifetime of an `rcl_node_t`, and accounts for its /// dependency on the lifetime of its `rcl_context_t` by ensuring that this /// dependency is [dropped after][1] the `rcl_node_t`. +/// Note: we capture the rcl_node_t returned from rcl_get_zero_initialized_node() +/// to guarantee that the node handle exists until we drop the NodeHandle +/// instance. This addresses an issue where previously the address of the variable +/// in the builder.rs was being used, and whose lifespan was (just) shorter than the +/// NodeHandle instance. /// /// [1]: pub(crate) struct NodeHandle { pub(crate) rcl_node: Mutex, pub(crate) context_handle: Arc, + /// In the humble distro, rcl is sensitive to the address of the rcl_node_t + /// object being moved (this issue seems to be gone in jazzy), so we need + /// to initialize the rcl_node_t in-place inside this struct. In the event + /// that the initialization fails (e.g. it was created with an invalid name) + /// we need to make sure that we do not call rcl_node_fini on it while + /// dropping the NodeHandle, so we keep track of successful initialization + /// with this variable. + /// + /// We may be able to restructure this in the future when we no longer need + /// to support Humble. + pub(crate) initialized: AtomicBool, } impl Drop for NodeHandle { fn drop(&mut self) { + if !self.initialized.load(std::sync::atomic::Ordering::Acquire) { + // The node was not correctly initialized, e.g. it was created with + // an invalid name, so we must not try to finalize it or else we + // will get undefined behavior. + return; + } + let _context_lock = self.context_handle.rcl_context.lock().unwrap(); let mut rcl_node = self.rcl_node.lock().unwrap(); let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of // global variables in the rmw implementation being unsafely modified during cleanup. unsafe { rcl_node_fini(&mut *rcl_node) }; @@ -440,6 +465,17 @@ impl Node { pub fn builder(context: &Context, node_name: &str) -> NodeBuilder { NodeBuilder::new(context, node_name) } + + /// Get the logger associated with this Node. + pub fn logger(&self) -> &Logger { + &self.logger + } +} + +impl<'a> ToLogParams<'a> for &'a Node { + fn to_log_params(self) -> LogParams<'a> { + self.logger().to_log_params() + } } // Helper used to implement call_string_getter(), but also used to get the FQN in the Node::new() @@ -514,4 +550,21 @@ mod tests { Ok(()) } + + #[test] + fn test_logger_name() -> Result<(), RclrsError> { + // Use helper to create 2 nodes for us + let graph = construct_test_graph("test_logger_name")?; + + assert_eq!( + graph.node1.logger().name(), + "test_logger_name.graph_test_node_1" + ); + assert_eq!( + graph.node2.logger().name(), + "test_logger_name.graph_test_node_2" + ); + + Ok(()) + } } diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 011d5d2f3..1e7a9fc63 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -1,11 +1,12 @@ use std::{ - ffi::CString, - sync::{Arc, Mutex}, + ffi::{CStr, CString}, + sync::{atomic::AtomicBool, Arc, Mutex}, }; use crate::{ - rcl_bindings::*, ClockType, Context, ContextHandle, Node, NodeHandle, ParameterInterface, - QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, + rcl_bindings::*, ClockType, Context, ContextHandle, Logger, Node, NodeHandle, + ParameterInterface, QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, + QOS_PROFILE_CLOCK, }; /// A builder for creating a [`Node`][1]. @@ -276,8 +277,13 @@ impl NodeBuilder { let rcl_node_options = self.create_rcl_node_options()?; let rcl_context = &mut *self.context.rcl_context.lock().unwrap(); - // SAFETY: Getting a zero-initialized value is always safe. - let mut rcl_node = unsafe { rcl_get_zero_initialized_node() }; + let handle = Arc::new(NodeHandle { + // SAFETY: Getting a zero-initialized value is always safe. + rcl_node: Mutex::new(unsafe { rcl_get_zero_initialized_node() }), + context_handle: Arc::clone(&self.context), + initialized: AtomicBool::new(false), + }); + unsafe { // SAFETY: // * The rcl_node is zero-initialized as mandated by this function. @@ -287,7 +293,7 @@ impl NodeBuilder { // global variables in the rmw implementation being unsafely modified during cleanup. let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); rcl_node_init( - &mut rcl_node, + &mut *handle.rcl_node.lock().unwrap(), node_name.as_ptr(), node_namespace.as_ptr(), rcl_context, @@ -296,10 +302,10 @@ impl NodeBuilder { .ok()?; }; - let handle = Arc::new(NodeHandle { - rcl_node: Mutex::new(rcl_node), - context_handle: Arc::clone(&self.context), - }); + handle + .initialized + .store(true, std::sync::atomic::Ordering::Release); + let parameter = { let rcl_node = handle.rcl_node.lock().unwrap(); ParameterInterface::new( @@ -308,6 +314,26 @@ impl NodeBuilder { &rcl_context.global_arguments, )? }; + + let logger_name = { + let rcl_node = handle.rcl_node.lock().unwrap(); + let logger_name_raw_ptr = unsafe { rcl_node_get_logger_name(&*rcl_node) }; + if logger_name_raw_ptr.is_null() { + "" + } else { + // SAFETY: rcl_node_get_logger_name will either return a nullptr + // if the provided node was invalid or provide a valid null-terminated + // const char* if the provided node was valid. We have already + // verified that it is not a nullptr. We are also preventing the + // pointed-to value from being modified while we view it by locking + // the mutex of rcl_node while we view it. This means all the + // safety conditions of CStr::from_ptr are met. + unsafe { CStr::from_ptr(logger_name_raw_ptr) } + .to_str() + .unwrap_or("") + } + }; + let node = Arc::new(Node { handle, clients_mtx: Mutex::new(vec![]), @@ -318,7 +344,9 @@ impl NodeBuilder { .clock_qos(self.clock_qos) .build(), parameter, + logger: Logger::new(logger_name)?, }); + node.time_source.attach_node(&node); if self.start_parameter_services { node.parameter.create_services(&node)?; diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 343fa61d3..639a38e38 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -487,12 +487,29 @@ mod tests { .unwrap(); let node_name = "test_publisher_names_and_types"; let node = Node::new(&context, node_name).unwrap(); - // Test that the graph has no publishers + + let check_rosout = |topics: HashMap>| { + // rosout shows up in humble and iron, even if the graph is empty + #[cfg(any(ros_distro = "humble", ros_distro = "iron"))] + { + assert_eq!(topics.len(), 1); + assert_eq!( + topics.get("/rosout").unwrap().first().unwrap(), + "rcl_interfaces/msg/Log" + ); + } + + // rosout does not automatically show up in jazzy when the graph is empty + #[cfg(any(ros_distro = "jazzy", ros_distro = "rolling"))] + { + assert_eq!(topics.len(), 0); + } + }; + let names_and_topics = node .get_publisher_names_and_types_by_node(node_name, "") .unwrap(); - - assert_eq!(names_and_topics.len(), 0); + check_rosout(names_and_topics); let num_publishers = node.count_publishers("/test").unwrap(); @@ -535,10 +552,8 @@ mod tests { assert_eq!(names_and_topics.len(), 0); - // Test that the graph has no topics let names_and_topics = node.get_topic_names_and_types().unwrap(); - - assert_eq!(names_and_topics.len(), 0); + check_rosout(names_and_topics); } #[test] diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index c7153ce01..3c49993b3 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -10,7 +10,9 @@ pub use value::*; use crate::vendor::rcl_interfaces::msg::rmw::{ParameterType, ParameterValue as RmwParameterValue}; -use crate::{call_string_getter_with_rcl_node, rcl_bindings::*, Node, RclrsError}; +use crate::{ + call_string_getter_with_rcl_node, rcl_bindings::*, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX, +}; use std::{ collections::{btree_map::Entry, BTreeMap}, fmt::Debug, @@ -760,6 +762,7 @@ impl ParameterInterface { global_arguments: &rcl_arguments_t, ) -> Result { let override_map = unsafe { + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); let fqn = call_string_getter_with_rcl_node(rcl_node, rcl_node_get_fully_qualified_name); resolve_parameter_overrides(&fqn, node_arguments, global_arguments)? }; @@ -882,7 +885,7 @@ mod tests { String::from("declared_int:=10"), ]) .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); // Declaring a parameter with a different type than what was overridden should return an // error @@ -940,7 +943,7 @@ mod tests { String::from("non_declared_string:='param'"), ]) .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); let overridden_int = node .declare_parameter("declared_int") @@ -1090,7 +1093,7 @@ mod tests { String::from("declared_int:=10"), ]) .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); // If a parameter was set as an override and as an undeclared parameter, the undeclared // value should get priority node.use_undeclared_parameters() @@ -1112,7 +1115,7 @@ mod tests { String::from("declared_int:=10"), ]) .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); { // Setting a parameter with an override let param = node @@ -1158,7 +1161,7 @@ mod tests { #[test] fn test_parameter_ranges() { let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); // Setting invalid ranges should fail let range = ParameterRange { lower: Some(10), @@ -1286,7 +1289,7 @@ mod tests { #[test] fn test_readonly_parameters() { let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); let param = node .declare_parameter("int_param") .default(100) @@ -1313,7 +1316,7 @@ mod tests { #[test] fn test_preexisting_value_error() { let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); node.use_undeclared_parameters() .set("int_param", 100) .unwrap(); @@ -1366,7 +1369,7 @@ mod tests { #[test] fn test_optional_parameter_apis() { let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); node.declare_parameter::("int_param") .optional() .unwrap(); diff --git a/rclrs/src/rcl_wrapper.h b/rclrs/src/rcl_wrapper.h index fe97cb4e5..ececf491f 100644 --- a/rclrs/src/rcl_wrapper.h +++ b/rclrs/src/rcl_wrapper.h @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index a6b600800..0be0c07ec 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -149,7 +149,11 @@ mod tests { #[test] fn time_source_default_clock() { - let node = create_node(&Context::new([]).unwrap(), "test_node").unwrap(); + let node = create_node( + &Context::new([]).unwrap(), + &format!("time_source_test_node_{}", line!()), + ) + .unwrap(); // Default clock should be above 0 (use_sim_time is default false) assert!(node.get_clock().now().nsec > 0); } @@ -162,7 +166,7 @@ mod tests { String::from("use_sim_time:=true"), ]) .unwrap(); - let node = create_node(&ctx, "test_node").unwrap(); + let node = create_node(&ctx, &format!("time_source_test_node_{}", line!())).unwrap(); // Default sim time value should be 0 (no message received) assert_eq!(node.get_clock().now().nsec, 0); } From 885f4156d4278f392d25f7b7fc5be7cb15ce94af Mon Sep 17 00:00:00 2001 From: Milan Vukov Date: Mon, 25 Nov 2024 22:49:33 +0100 Subject: [PATCH 15/20] Add Publisher::can_loan_msgs (#434) * Add Publisher::can_loan_msgs * Fix formatting --- rclrs/src/publisher.rs | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 2935ca322..b1cdd93b9 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -229,6 +229,11 @@ where msg_ptr: msg_ptr as *mut T, }) } + + /// Returns true if message loans are possible, false otherwise. + pub fn can_loan_messages(&self) -> bool { + unsafe { rcl_publisher_can_loan_messages(&*self.handle.rcl_publisher.lock().unwrap()) } + } } /// Convenience trait for [`Publisher::publish`]. From a604ad7053f73b112c8142ca0a0c7919f4edb5dc Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Tue, 26 Nov 2024 13:32:03 +0100 Subject: [PATCH 16/20] Installs rust toolchain 1.75 in docker workflow. See #420. (#436) Signed-off-by: Agustin Alba Chicar --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index b148eafe3..d117d5c0f 100644 --- a/Dockerfile +++ b/Dockerfile @@ -12,7 +12,7 @@ RUN apt-get update && apt-get install -y \ && rm -rf /var/lib/apt/lists/* # Install Rust and the cargo-ament-build plugin -RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- --default-toolchain 1.74.0 -y +RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- --default-toolchain 1.75.0 -y ENV PATH=/root/.cargo/bin:$PATH RUN cargo install cargo-ament-build From 3eba2b03e9730739721c10e9efd26034e021ea4b Mon Sep 17 00:00:00 2001 From: Grey Date: Mon, 2 Dec 2024 21:42:36 +0800 Subject: [PATCH 17/20] Update for clippy 1.83 (#441) * Update for clippy 1.83 Signed-off-by: Michael X. Grey * Update clippy for cfg(test) as well Signed-off-by: Michael X. Grey * Exclude dependencies from clippy test Signed-off-by: Michael X. Grey * Fix clippy for rosidl_runtime_rs Signed-off-by: Michael X. Grey --------- Signed-off-by: Michael X. Grey --- .github/workflows/rust-minimal.yml | 4 ++-- .github/workflows/rust-stable.yml | 4 ++-- rclrs/src/logging/logging_configuration.rs | 4 ++-- rclrs/src/parameter.rs | 6 +++--- rclrs/src/publisher/loaned_message.rs | 12 ++++++------ rclrs/src/subscription/readonly_loaned_message.rs | 8 ++++---- rosidl_runtime_rs/src/string/serde.rs | 2 +- rosidl_runtime_rs/src/traits.rs | 5 +++-- 8 files changed, 23 insertions(+), 22 deletions(-) diff --git a/.github/workflows/rust-minimal.yml b/.github/workflows/rust-minimal.yml index 9b1a5bb49..66fc3c038 100644 --- a/.github/workflows/rust-minimal.yml +++ b/.github/workflows/rust-minimal.yml @@ -93,9 +93,9 @@ jobs: echo "Running clippy in $path" # Run clippy for all features except generate_docs (needed for docs.rs) if [ "$(basename $path)" = "rclrs" ]; then - cargo clippy --all-targets -F default,dyn_msg -- -D warnings + cargo clippy --no-deps --all-targets -F default,dyn_msg -- -D warnings else - cargo clippy --all-targets --all-features -- -D warnings + cargo clippy --no-deps --all-targets --all-features -- -D warnings fi cd - done diff --git a/.github/workflows/rust-stable.yml b/.github/workflows/rust-stable.yml index 6dc395496..944b9da27 100644 --- a/.github/workflows/rust-stable.yml +++ b/.github/workflows/rust-stable.yml @@ -93,9 +93,9 @@ jobs: echo "Running clippy in $path" # Run clippy for all features except generate_docs (needed for docs.rs) if [ "$(basename $path)" = "rclrs" ]; then - cargo clippy --all-targets -F default,dyn_msg -- -D warnings + cargo clippy --no-deps --all-targets -F default,dyn_msg -- -D warnings else - cargo clippy --all-targets --all-features -- -D warnings + cargo clippy --no-deps --all-targets --all-features -- -D warnings fi cd - done diff --git a/rclrs/src/logging/logging_configuration.rs b/rclrs/src/logging/logging_configuration.rs index 1012608ec..7b57ad60f 100644 --- a/rclrs/src/logging/logging_configuration.rs +++ b/rclrs/src/logging/logging_configuration.rs @@ -103,7 +103,7 @@ pub(crate) mod log_handler { pub(crate) message: Cow<'a, str>, } - impl<'a> LogEntry<'a> { + impl LogEntry<'_> { /// Change the entry from something borrowed into something owned pub(crate) fn into_owned(self) -> LogEntry<'static> { LogEntry { @@ -124,7 +124,7 @@ pub(crate) mod log_handler { pub line_number: usize, } - impl<'a> LogLocation<'a> { + impl LogLocation<'_> { pub(crate) fn into_owned(self) -> LogLocation<'static> { LogLocation { function_name: Cow::Owned(self.function_name.into_owned()), diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index 3c49993b3..2a0829eac 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -195,7 +195,7 @@ impl<'a, T: ParameterVariant> ParameterBuilder<'a, T> { } } -impl<'a, T> ParameterBuilder<'a, Arc<[T]>> +impl ParameterBuilder<'_, Arc<[T]>> where Arc<[T]>: ParameterVariant, { @@ -206,7 +206,7 @@ where } } -impl<'a> ParameterBuilder<'a, Arc<[Arc]>> { +impl ParameterBuilder<'_, Arc<[Arc]>> { /// Sets the default for the parameter from a string-like array. pub fn default_string_array(mut self, default_value: U) -> Self where @@ -679,7 +679,7 @@ impl std::fmt::Display for DeclarationError { impl std::error::Error for DeclarationError {} -impl<'a> Parameters<'a> { +impl Parameters<'_> { /// Tries to read a parameter of the requested type. /// /// Returns `Some(T)` if a parameter of the requested type exists, `None` otherwise. diff --git a/rclrs/src/publisher/loaned_message.rs b/rclrs/src/publisher/loaned_message.rs index c03fc300f..7d29122dc 100644 --- a/rclrs/src/publisher/loaned_message.rs +++ b/rclrs/src/publisher/loaned_message.rs @@ -22,7 +22,7 @@ where pub(super) publisher: &'a Publisher, } -impl<'a, T> Deref for LoanedMessage<'a, T> +impl Deref for LoanedMessage<'_, T> where T: RmwMessage, { @@ -33,7 +33,7 @@ where } } -impl<'a, T> DerefMut for LoanedMessage<'a, T> +impl DerefMut for LoanedMessage<'_, T> where T: RmwMessage, { @@ -43,7 +43,7 @@ where } } -impl<'a, T> Drop for LoanedMessage<'a, T> +impl Drop for LoanedMessage<'_, T> where T: RmwMessage, { @@ -66,11 +66,11 @@ where // 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<'a, T> Send for LoanedMessage<'a, T> where T: RmwMessage {} +unsafe impl Send for LoanedMessage<'_, T> where T: RmwMessage {} // SAFETY: There is no interior mutability in this type. All mutation happens through &mut references. -unsafe impl<'a, T> Sync for LoanedMessage<'a, T> where T: RmwMessage {} +unsafe impl Sync for LoanedMessage<'_, T> where T: RmwMessage {} -impl<'a, T> LoanedMessage<'a, T> +impl LoanedMessage<'_, T> where T: RmwMessage, { diff --git a/rclrs/src/subscription/readonly_loaned_message.rs b/rclrs/src/subscription/readonly_loaned_message.rs index df4ad6a5b..c6f52e280 100644 --- a/rclrs/src/subscription/readonly_loaned_message.rs +++ b/rclrs/src/subscription/readonly_loaned_message.rs @@ -22,7 +22,7 @@ where pub(super) subscription: &'a Subscription, } -impl<'a, T> Deref for ReadOnlyLoanedMessage<'a, T> +impl Deref for ReadOnlyLoanedMessage<'_, T> where T: Message, { @@ -32,7 +32,7 @@ where } } -impl<'a, T> Drop for ReadOnlyLoanedMessage<'a, T> +impl Drop for ReadOnlyLoanedMessage<'_, T> where T: Message, { @@ -50,9 +50,9 @@ where // 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<'a, T> Send for ReadOnlyLoanedMessage<'a, T> where T: Message {} +unsafe impl Send for ReadOnlyLoanedMessage<'_, T> where T: Message {} // SAFETY: This type has no interior mutability, in fact it has no mutability at all. -unsafe impl<'a, T> Sync for ReadOnlyLoanedMessage<'a, T> where T: Message {} +unsafe impl Sync for ReadOnlyLoanedMessage<'_, T> where T: Message {} #[cfg(test)] mod tests { diff --git a/rosidl_runtime_rs/src/string/serde.rs b/rosidl_runtime_rs/src/string/serde.rs index fe68661b5..0ded3fd4a 100644 --- a/rosidl_runtime_rs/src/string/serde.rs +++ b/rosidl_runtime_rs/src/string/serde.rs @@ -13,7 +13,7 @@ use super::{ struct StringVisitor; struct WStringVisitor; -impl<'de> Visitor<'de> for StringVisitor { +impl Visitor<'_> for StringVisitor { type Value = String; fn expecting(&self, formatter: &mut fmt::Formatter) -> fmt::Result { diff --git a/rosidl_runtime_rs/src/traits.rs b/rosidl_runtime_rs/src/traits.rs index 1e0908544..61d8c2392 100644 --- a/rosidl_runtime_rs/src/traits.rs +++ b/rosidl_runtime_rs/src/traits.rs @@ -124,10 +124,11 @@ pub trait RmwMessage: Clone + Debug + Default + Send + Sync + Message { /// /// Memory ownership by C is achieved by calling `init()` when any string or sequence is created, /// as well as in the `Default` impl for messages. +/// /// User code can still create messages explicitly, which will not call `init()`, but this is not a -/// problem, since nothing is allocated this way. +/// problem, since nothing is allocated this way. +/// /// The `Drop` impl for any sequence or string will call `fini()`. - pub trait Message: Clone + Debug + Default + 'static + Send + Sync { /// The corresponding RMW-native message type. type RmwMsg: RmwMessage; From 506a60cb40587b5be3ad30ca7457f3d806816757 Mon Sep 17 00:00:00 2001 From: Javier Balloffet Date: Mon, 2 Dec 2024 16:18:41 +0100 Subject: [PATCH 18/20] Fix GHA status labels (#438) Signed-off-by: Javier Balloffet --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 7fb17a8db..c882b5a5a 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,9 @@ ROS 2 for Rust ============== -| Target | Status | -|----------|--------| -| **Ubuntu 20.04** | [![Build Status](https://github.com/ros2-rust/ros2_rust/actions/workflows/rust.yml/badge.svg?branch=main)](https://github.com/ros2-rust/ros2_rust/actions/workflows/rust.yml?branch=main) | +[![Minimal Version Status](https://github.com/ros2-rust/ros2_rust/actions/workflows/rust-minimal.yml/badge.svg?branch=main)](https://github.com/ros2-rust/ros2_rust/actions/workflows/rust-minimal.yml) +[![Stable CI Status](https://github.com/ros2-rust/ros2_rust/actions/workflows/rust-stable.yml/badge.svg?branch=main)](https://github.com/ros2-rust/ros2_rust/actions/workflows/rust-stable.yml) +[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) Introduction ------------ From 6ac2ae1e57623311069e68822c314f9ef138533c Mon Sep 17 00:00:00 2001 From: Javier Balloffet Date: Mon, 2 Dec 2024 18:00:09 +0100 Subject: [PATCH 19/20] Fix GHA set-output command warning (#439) Signed-off-by: Javier Balloffet --- .github/workflows/rust-minimal.yml | 6 +++++- .github/workflows/rust-stable.yml | 6 +++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/.github/workflows/rust-minimal.yml b/.github/workflows/rust-minimal.yml index 66fc3c038..f266b02c6 100644 --- a/.github/workflows/rust-minimal.yml +++ b/.github/workflows/rust-minimal.yml @@ -45,7 +45,11 @@ jobs: - name: Search packages in this repository id: list_packages run: | - echo ::set-output name=package_list::$(colcon list --names-only) + { + echo 'package_list<> "$GITHUB_OUTPUT" - name: Setup ROS environment uses: ros-tooling/setup-ros@v0.7 diff --git a/.github/workflows/rust-stable.yml b/.github/workflows/rust-stable.yml index 944b9da27..4cc44d4db 100644 --- a/.github/workflows/rust-stable.yml +++ b/.github/workflows/rust-stable.yml @@ -45,7 +45,11 @@ jobs: - name: Search packages in this repository id: list_packages run: | - echo ::set-output name=package_list::$(colcon list --names-only) + { + echo 'package_list<> "$GITHUB_OUTPUT" - name: Setup ROS environment uses: ros-tooling/setup-ros@v0.7 From f706824e4e0349fa6b09e25109eb633ce1a57e7f Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Wed, 4 Dec 2024 14:29:05 +0100 Subject: [PATCH 20/20] chore: remove references to installing cargo-ament-build via cargo (#442) Signed-off-by: Esteve Fernandez --- .github/workflows/rust-minimal.yml | 4 ---- .github/workflows/rust-stable.yml | 4 ---- Dockerfile | 3 +-- README.md | 1 - docs/building.md | 1 - 5 files changed, 1 insertion(+), 12 deletions(-) diff --git a/.github/workflows/rust-minimal.yml b/.github/workflows/rust-minimal.yml index f266b02c6..177e667e5 100644 --- a/.github/workflows/rust-minimal.yml +++ b/.github/workflows/rust-minimal.yml @@ -76,10 +76,6 @@ jobs: cd - done - - name: Install cargo-ament-build - run: | - cargo install --debug cargo-ament-build - - name: Build and test id: build uses: ros-tooling/action-ros-ci@v0.3 diff --git a/.github/workflows/rust-stable.yml b/.github/workflows/rust-stable.yml index 4cc44d4db..4f6bf2300 100644 --- a/.github/workflows/rust-stable.yml +++ b/.github/workflows/rust-stable.yml @@ -76,10 +76,6 @@ jobs: cd - done - - name: Install cargo-ament-build - run: | - cargo install --debug cargo-ament-build - - name: Build and test id: build uses: ros-tooling/action-ros-ci@v0.3 diff --git a/Dockerfile b/Dockerfile index d117d5c0f..ede9821ea 100644 --- a/Dockerfile +++ b/Dockerfile @@ -11,10 +11,9 @@ RUN apt-get update && apt-get install -y \ python3-pip \ && rm -rf /var/lib/apt/lists/* -# Install Rust and the cargo-ament-build plugin +# Install Rust RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- --default-toolchain 1.75.0 -y ENV PATH=/root/.cargo/bin:$PATH -RUN cargo install cargo-ament-build RUN pip install --upgrade pytest diff --git a/README.md b/README.md index c882b5a5a..cee8a9b8e 100644 --- a/README.md +++ b/README.md @@ -37,7 +37,6 @@ Here are the steps for building the `ros2_rust` examples in a vanilla Ubuntu Foc # Assuming you installed the minimal version of ROS 2, you need these additional packages: sudo apt install -y git libclang-dev python3-pip python3-vcstool # libclang-dev is required by bindgen # Install these plugins for cargo and colcon: -cargo install --debug cargo-ament-build # --debug is faster to install pip install git+https://github.com/colcon/colcon-cargo.git pip install git+https://github.com/colcon/colcon-ros-cargo.git diff --git a/docs/building.md b/docs/building.md index 379f524af..610187cee 100644 --- a/docs/building.md +++ b/docs/building.md @@ -43,7 +43,6 @@ The exact steps may differ between platforms, but as an example, here is how you # Assuming you installed the minimal version of ROS 2, you need these additional packages: sudo apt install -y git libclang-dev python3-pip python3-vcstool # libclang-dev is required by bindgen # Install these plugins for cargo and colcon: -cargo install cargo-ament-build pip install git+https://github.com/colcon/colcon-cargo.git pip install git+https://github.com/colcon/colcon-ros-cargo.git ```