diff --git a/third-party/realdds/include/realdds/topics/ros2/describe-parameters-msg.h b/third-party/realdds/include/realdds/topics/ros2/describe-parameters-msg.h new file mode 100644 index 0000000000..f3f9e98aea --- /dev/null +++ b/third-party/realdds/include/realdds/topics/ros2/describe-parameters-msg.h @@ -0,0 +1,114 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. +#pragma once + +#include + +#include +#include + +#include +#include +#include + + +namespace rcl_interfaces { +namespace srv { +class DescribeParameters_RequestPubSubType; +class DescribeParameters_ResponsePubSubType; +} // namespace srv +} // namespace rcl_interfaces + + +namespace realdds { + + +class dds_participant; +class dds_topic; +class dds_topic_reader; +class dds_topic_writer; + + +namespace topics { +namespace ros2 { + + +class describe_parameters_request_msg +{ + rcl_interfaces::srv::DescribeParameters_Request _raw; + +public: + using type = rcl_interfaces::srv::DescribeParameters_RequestPubSubType; + + void clear() { _raw.names().clear(); } + std::vector< std::string > const & names() const { return _raw.names(); } + void add( std::string const & name ) { _raw.names().push_back( name ); } + + bool is_valid() const { return ! names().empty(); } + void invalidate() { clear(); } + + + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + char const * topic_name ); + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + std::string const & topic_name ) + { + return create_topic( participant, topic_name.c_str() ); + } + + // This helper method will take the next sample from a reader. + // + // Returns true if successful. Make sure you still check is_valid() in case the sample info isn't! + // Returns false if no more data is available. + // Will throw if an unexpected error occurs. + // + //Note - copies the data. + //TODO - add an API for a function that loans the data and enables the user to free it later. + static bool take_next( dds_topic_reader &, + describe_parameters_request_msg * output, + dds_sample * optional_sample = nullptr ); +}; + + +class describe_parameters_response_msg +{ + rcl_interfaces::srv::DescribeParameters_Response _raw; + +public: + using type = rcl_interfaces::srv::DescribeParameters_ResponsePubSubType; + + using descriptor_type = rcl_interfaces::msg::ParameterDescriptor; + + std::vector< descriptor_type > const & descriptors() const { return _raw.descriptors(); } + bool is_valid() const { return ! descriptors().empty(); } + void invalidate() { _raw.descriptors().clear(); } + + + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + char const * topic_name ); + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + std::string const & topic_name ) + { + return create_topic( participant, topic_name.c_str() ); + } + + // Returns some unique (to the writer) identifier for the sample that was sent, or 0 if unsuccessful + dds_sequence_number write_to( dds_topic_writer & ); + + // This helper method will take the next sample from a reader. + // + // Returns true if successful. Make sure you still check is_valid() in case the sample info isn't! + // Returns false if no more data is available. + // Will throw if an unexpected error occurs. + // + //Note - copies the data. + //TODO - add an API for a function that loans the data and enables the user to free it later. + static bool take_next( dds_topic_reader &, + describe_parameters_response_msg * output, + dds_sample * optional_sample = nullptr ); +}; + + +} // namespace ros2 +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/include/realdds/topics/ros2/get-parameters-msg.h b/third-party/realdds/include/realdds/topics/ros2/get-parameters-msg.h new file mode 100644 index 0000000000..0114b1c424 --- /dev/null +++ b/third-party/realdds/include/realdds/topics/ros2/get-parameters-msg.h @@ -0,0 +1,114 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. +#pragma once + +#include + +#include +#include + +#include +#include +#include + + +namespace rcl_interfaces { +namespace srv { +class GetParameters_RequestPubSubType; +class GetParameters_ResponsePubSubType; +} // namespace srv +} // namespace rcl_interfaces + + +namespace realdds { + + +class dds_participant; +class dds_topic; +class dds_topic_reader; +class dds_topic_writer; + + +namespace topics { +namespace ros2 { + + +class get_parameters_request_msg +{ + rcl_interfaces::srv::GetParameters_Request _raw; + +public: + using type = rcl_interfaces::srv::GetParameters_RequestPubSubType; + + void clear() { _raw.names().clear(); } + std::vector< std::string > const & names() const { return _raw.names(); } + void add( std::string const & name ) { _raw.names().push_back( name ); } + + bool is_valid() const { return ! names().empty(); } + void invalidate() { clear(); } + + + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + char const * topic_name ); + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + std::string const & topic_name ) + { + return create_topic( participant, topic_name.c_str() ); + } + + // This helper method will take the next sample from a reader. + // + // Returns true if successful. Make sure you still check is_valid() in case the sample info isn't! + // Returns false if no more data is available. + // Will throw if an unexpected error occurs. + // + //Note - copies the data. + //TODO - add an API for a function that loans the data and enables the user to free it later. + static bool take_next( dds_topic_reader &, + get_parameters_request_msg * output, + dds_sample * optional_sample = nullptr ); +}; + + +class get_parameters_response_msg +{ + rcl_interfaces::srv::GetParameters_Response _raw; + +public: + using type = rcl_interfaces::srv::GetParameters_ResponsePubSubType; + + using value_type = rcl_interfaces::msg::ParameterValue; + + std::vector< value_type > const & values() const { return _raw.values(); } + void add( value_type const & value ) { _raw.values().push_back( value ); } + bool is_valid() const { return ! values().empty(); } + void invalidate() { _raw.values().clear(); } + + // Returns some unique (to the writer) identifier for the sample that was sent, or 0 if unsuccessful + dds_sequence_number write_to( dds_topic_writer & ); + + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + char const * topic_name ); + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + std::string const & topic_name ) + { + return create_topic( participant, topic_name.c_str() ); + } + + // This helper method will take the next sample from a reader. + // + // Returns true if successful. Make sure you still check is_valid() in case the sample info isn't! + // Returns false if no more data is available. + // Will throw if an unexpected error occurs. + // + //Note - copies the data. + //TODO - add an API for a function that loans the data and enables the user to free it later. + static bool take_next( dds_topic_reader &, + get_parameters_response_msg * output, + dds_sample * optional_sample = nullptr ); +}; + + +} // namespace ros2 +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/include/realdds/topics/ros2/list-parameters-msg.h b/third-party/realdds/include/realdds/topics/ros2/list-parameters-msg.h new file mode 100644 index 0000000000..3f52403757 --- /dev/null +++ b/third-party/realdds/include/realdds/topics/ros2/list-parameters-msg.h @@ -0,0 +1,122 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. +#pragma once + +#include + +#include +#include + +#include +#include +#include + + +namespace rcl_interfaces { +namespace srv { +class ListParameters_RequestPubSubType; +class ListParameters_ResponsePubSubType; +} // namespace srv +} // namespace rcl_interfaces + + +namespace realdds { + + +class dds_participant; +class dds_topic; +class dds_topic_reader; +class dds_topic_writer; + + +namespace topics { +namespace ros2 { + + +class list_parameters_request_msg +{ + rcl_interfaces::srv::ListParameters_Request _raw; + +public: + using type = rcl_interfaces::srv::ListParameters_RequestPubSubType; + + static const auto DEPTH_RECURSIVE = rcl_interfaces::srv::ListParameters_Request_Constants::DEPTH_RECURSIVE; + + // Relative depth from given prefixes + // Use DEPTH_RECURSIVE (the default) to get the recursive parameters and prefixes for each prefix; unlimited depth + uint64_t depth() const { return _raw.depth(); } + void depth( uint64_t d ) { _raw.depth( d ); } + + // If the prefixes are empty, returns all parameters + + void clear() { _raw.prefixes().clear(); } + std::vector< std::string > const & prefixes() const { return _raw.prefixes(); } + void add( std::string const & prefix ) { _raw.prefixes().push_back( prefix ); } + + bool is_valid() const { return depth() != 0xBADF00D; } + void invalidate() { depth( 0xBADF00D ); } + + + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + char const * topic_name ); + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + std::string const & topic_name ) + { + return create_topic( participant, topic_name.c_str() ); + } + + // This helper method will take the next sample from a reader. + // + // Returns true if successful. Make sure you still check is_valid() in case the sample info isn't! + // Returns false if no more data is available. + // Will throw if an unexpected error occurs. + // + //Note - copies the data. + //TODO - add an API for a function that loans the data and enables the user to free it later. + static bool take_next( dds_topic_reader &, + list_parameters_request_msg * output, + dds_sample * optional_sample = nullptr ); +}; + + +class list_parameters_response_msg +{ + rcl_interfaces::srv::ListParameters_Response _raw; + +public: + using type = rcl_interfaces::srv::ListParameters_ResponsePubSubType; + + std::vector< std::string > const & names() const { return _raw.result().names(); } + std::vector< std::string > const & prefixes() const { return _raw.result().prefixes(); } + bool is_valid() const { return true; } + void clear() { _raw.result().names().clear(); } + + + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + char const * topic_name ); + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + std::string const & topic_name ) + { + return create_topic( participant, topic_name.c_str() ); + } + + // Returns some unique (to the writer) identifier for the sample that was sent, or 0 if unsuccessful + dds_sequence_number write_to( dds_topic_writer & ); + + // This helper method will take the next sample from a reader. + // + // Returns true if successful. Make sure you still check is_valid() in case the sample info isn't! + // Returns false if no more data is available. + // Will throw if an unexpected error occurs. + // + //Note - copies the data. + //TODO - add an API for a function that loans the data and enables the user to free it later. + static bool take_next( dds_topic_reader &, + list_parameters_response_msg * output, + dds_sample * optional_sample = nullptr ); +}; + + +} // namespace ros2 +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/include/realdds/topics/ros2/set-parameters-msg.h b/third-party/realdds/include/realdds/topics/ros2/set-parameters-msg.h new file mode 100644 index 0000000000..a4b73c3ef4 --- /dev/null +++ b/third-party/realdds/include/realdds/topics/ros2/set-parameters-msg.h @@ -0,0 +1,117 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. +#pragma once + +#include + +#include +#include +#include + +#include +#include +#include + + +namespace rcl_interfaces { +namespace srv { +class SetParameters_RequestPubSubType; +class SetParameters_ResponsePubSubType; +} // namespace srv +} // namespace rcl_interfaces + + +namespace realdds { + + +class dds_participant; +class dds_topic; +class dds_topic_reader; +class dds_topic_writer; + + +namespace topics { +namespace ros2 { + + +class set_parameters_request_msg +{ + rcl_interfaces::srv::SetParameters_Request _raw; + +public: + using type = rcl_interfaces::srv::SetParameters_RequestPubSubType; + + using parameter_type = rcl_interfaces::msg::Parameter; + + void clear() { _raw.parameters().clear(); } + std::vector< parameter_type > const & parameters() const { return _raw.parameters(); } + void add( parameter_type const & parameter ) { _raw.parameters().push_back( parameter ); } + + bool is_valid() const { return ! parameters().empty(); } + void invalidate() { clear(); } + + + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + char const * topic_name ); + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + std::string const & topic_name ) + { + return create_topic( participant, topic_name.c_str() ); + } + + // This helper method will take the next sample from a reader. + // + // Returns true if successful. Make sure you still check is_valid() in case the sample info isn't! + // Returns false if no more data is available. + // Will throw if an unexpected error occurs. + // + //Note - copies the data. + //TODO - add an API for a function that loans the data and enables the user to free it later. + static bool take_next( dds_topic_reader &, + set_parameters_request_msg * output, + dds_sample * optional_sample = nullptr ); +}; + + +class set_parameters_response_msg +{ + rcl_interfaces::srv::SetParameters_Response _raw; + +public: + using type = rcl_interfaces::srv::SetParameters_ResponsePubSubType; + + using result_type = rcl_interfaces::msg::SetParametersResult; + + std::vector< result_type > const & results() const { return _raw.results(); } + bool is_valid() const { return ! results().empty(); } + void invalidate() { _raw.results().clear(); } + + + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + char const * topic_name ); + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, + std::string const & topic_name ) + { + return create_topic( participant, topic_name.c_str() ); + } + + // Returns some unique (to the writer) identifier for the sample that was sent, or 0 if unsuccessful + dds_sequence_number write_to( dds_topic_writer & ); + + // This helper method will take the next sample from a reader. + // + // Returns true if successful. Make sure you still check is_valid() in case the sample info isn't! + // Returns false if no more data is available. + // Will throw if an unexpected error occurs. + // + //Note - copies the data. + //TODO - add an API for a function that loans the data and enables the user to free it later. + static bool take_next( dds_topic_reader &, + set_parameters_response_msg * output, + dds_sample * optional_sample = nullptr ); +}; + + +} // namespace ros2 +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2-describe-parameters-request-msg.cpp b/third-party/realdds/src/topics/ros2-describe-parameters-request-msg.cpp new file mode 100644 index 0000000000..cfd4cab27e --- /dev/null +++ b/third-party/realdds/src/topics/ros2-describe-parameters-request-msg.cpp @@ -0,0 +1,60 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#include +#include +#include + +#include +#include +#include + +#include +#include + + +namespace realdds { +namespace topics { +namespace ros2 { + + +/*static*/ std::shared_ptr< dds_topic > +describe_parameters_request_msg::create_topic( std::shared_ptr< dds_participant > const & participant, char const * topic_name ) +{ + return std::make_shared< dds_topic >( participant, + eprosima::fastdds::dds::TypeSupport( new describe_parameters_request_msg::type ), + topic_name ); +} + + +/*static*/ bool +describe_parameters_request_msg::take_next( dds_topic_reader & reader, describe_parameters_request_msg * output, dds_sample * sample ) +{ + describe_parameters_request_msg output_; + if( ! output ) + output = &output_; // use the local copy if the user hasn't provided their own + dds_sample sample_; + if( ! sample ) + sample = &sample_; // use the local copy if the user hasn't provided their own + auto status = reader->take_next_sample( &output->_raw, sample ); + if( status == ReturnCode_t::RETCODE_OK ) + { + // Only samples for which valid_data is true should be accessed + // valid_data indicates that the instance is still ALIVE and the `take` return an + // updated sample + if( ! sample->valid_data ) + output->invalidate(); + + return true; + } + if ( status == ReturnCode_t::RETCODE_NO_DATA ) + { + // This is an expected return code and is not an error + return false; + } + DDS_API_CALL_THROW( "describe_parameters_request_msg::take_next", status ); +} + +} // namespace ros2 +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2-describe-parameters-response-msg.cpp b/third-party/realdds/src/topics/ros2-describe-parameters-response-msg.cpp new file mode 100644 index 0000000000..ffd3fec563 --- /dev/null +++ b/third-party/realdds/src/topics/ros2-describe-parameters-response-msg.cpp @@ -0,0 +1,78 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + + +namespace realdds { +namespace topics { +namespace ros2 { + + +/*static*/ std::shared_ptr< dds_topic > +describe_parameters_response_msg::create_topic( std::shared_ptr< dds_participant > const & participant, char const * topic_name ) +{ + return std::make_shared< dds_topic >( participant, + eprosima::fastdds::dds::TypeSupport( new describe_parameters_response_msg::type ), + topic_name ); +} + + +/*static*/ bool +describe_parameters_response_msg::take_next( dds_topic_reader & reader, describe_parameters_response_msg * output, dds_sample * sample ) +{ + describe_parameters_response_msg output_; + if( ! output ) + output = &output_; // use the local copy if the user hasn't provided their own + dds_sample sample_; + if( ! sample ) + sample = &sample_; // use the local copy if the user hasn't provided their own + auto status = reader->take_next_sample( &output->_raw, sample ); + if( status == ReturnCode_t::RETCODE_OK ) + { + // Only samples for which valid_data is true should be accessed + // valid_data indicates that the instance is still ALIVE and the `take` return an + // updated sample + if( ! sample->valid_data ) + output->invalidate(); + + return true; + } + if ( status == ReturnCode_t::RETCODE_NO_DATA ) + { + // This is an expected return code and is not an error + return false; + } + DDS_API_CALL_THROW( "describe_parameters_response_msg::take_next", status ); +} + + +dds_sequence_number describe_parameters_response_msg::write_to( dds_topic_writer & writer ) +{ + eprosima::fastrtps::rtps::WriteParams params; + bool success = DDS_API_CALL( writer.get()->write( &_raw, params ) ); + if( ! success ) + { + LOG_ERROR( "Error writing message" ); + return 0; + } + // The params will contain, after the write, the sequence number (incremented automatically) for the sample that was + // sent. The source_timestamp is always INVALID for some reason. + return params.sample_identity().sequence_number().to64long(); +} + + +} // namespace ros2 +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2-get-parameters-request-msg.cpp b/third-party/realdds/src/topics/ros2-get-parameters-request-msg.cpp new file mode 100644 index 0000000000..f5066244c9 --- /dev/null +++ b/third-party/realdds/src/topics/ros2-get-parameters-request-msg.cpp @@ -0,0 +1,60 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#include +#include +#include + +#include +#include +#include + +#include +#include + + +namespace realdds { +namespace topics { +namespace ros2 { + + +/*static*/ std::shared_ptr< dds_topic > +get_parameters_request_msg::create_topic( std::shared_ptr< dds_participant > const & participant, char const * topic_name ) +{ + return std::make_shared< dds_topic >( participant, + eprosima::fastdds::dds::TypeSupport( new get_parameters_request_msg::type ), + topic_name ); +} + + +/*static*/ bool +get_parameters_request_msg::take_next( dds_topic_reader & reader, get_parameters_request_msg * output, dds_sample * sample ) +{ + get_parameters_request_msg output_; + if( ! output ) + output = &output_; // use the local copy if the user hasn't provided their own + dds_sample sample_; + if( ! sample ) + sample = &sample_; // use the local copy if the user hasn't provided their own + auto status = reader->take_next_sample( &output->_raw, sample ); + if( status == ReturnCode_t::RETCODE_OK ) + { + // Only samples for which valid_data is true should be accessed + // valid_data indicates that the instance is still ALIVE and the `take` return an + // updated sample + if( ! sample->valid_data ) + output->invalidate(); + + return true; + } + if ( status == ReturnCode_t::RETCODE_NO_DATA ) + { + // This is an expected return code and is not an error + return false; + } + DDS_API_CALL_THROW( "get_parameters_request_msg::take_next", status ); +} + +} // namespace ros2 +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2-get-parameters-response-msg.cpp b/third-party/realdds/src/topics/ros2-get-parameters-response-msg.cpp new file mode 100644 index 0000000000..347148bfa7 --- /dev/null +++ b/third-party/realdds/src/topics/ros2-get-parameters-response-msg.cpp @@ -0,0 +1,78 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + + +namespace realdds { +namespace topics { +namespace ros2 { + + +/*static*/ std::shared_ptr< dds_topic > +get_parameters_response_msg::create_topic( std::shared_ptr< dds_participant > const & participant, char const * topic_name ) +{ + return std::make_shared< dds_topic >( participant, + eprosima::fastdds::dds::TypeSupport( new get_parameters_response_msg::type ), + topic_name ); +} + + +/*static*/ bool +get_parameters_response_msg::take_next( dds_topic_reader & reader, get_parameters_response_msg * output, dds_sample * sample ) +{ + get_parameters_response_msg output_; + if( ! output ) + output = &output_; // use the local copy if the user hasn't provided their own + dds_sample sample_; + if( ! sample ) + sample = &sample_; // use the local copy if the user hasn't provided their own + auto status = reader->take_next_sample( &output->_raw, sample ); + if( status == ReturnCode_t::RETCODE_OK ) + { + // Only samples for which valid_data is true should be accessed + // valid_data indicates that the instance is still ALIVE and the `take` return an + // updated sample + if( ! sample->valid_data ) + output->invalidate(); + + return true; + } + if ( status == ReturnCode_t::RETCODE_NO_DATA ) + { + // This is an expected return code and is not an error + return false; + } + DDS_API_CALL_THROW( "get_parameters_response_msg::take_next", status ); +} + + +dds_sequence_number get_parameters_response_msg::write_to( dds_topic_writer & writer ) +{ + eprosima::fastrtps::rtps::WriteParams params; + bool success = DDS_API_CALL( writer.get()->write( &_raw, params ) ); + if( ! success ) + { + LOG_ERROR( "Error writing message" ); + return 0; + } + // The params will contain, after the write, the sequence number (incremented automatically) for the sample that was + // sent. The source_timestamp is always INVALID for some reason. + return params.sample_identity().sequence_number().to64long(); +} + + +} // namespace ros2 +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2-list-parameters-request-msg.cpp b/third-party/realdds/src/topics/ros2-list-parameters-request-msg.cpp new file mode 100644 index 0000000000..ca7de2aa5a --- /dev/null +++ b/third-party/realdds/src/topics/ros2-list-parameters-request-msg.cpp @@ -0,0 +1,60 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#include +#include +#include + +#include +#include +#include + +#include +#include + + +namespace realdds { +namespace topics { +namespace ros2 { + + +/*static*/ std::shared_ptr< dds_topic > +list_parameters_request_msg::create_topic( std::shared_ptr< dds_participant > const & participant, char const * topic_name ) +{ + return std::make_shared< dds_topic >( participant, + eprosima::fastdds::dds::TypeSupport( new list_parameters_request_msg::type ), + topic_name ); +} + + +/*static*/ bool +list_parameters_request_msg::take_next( dds_topic_reader & reader, list_parameters_request_msg * output, dds_sample * sample ) +{ + list_parameters_request_msg output_; + if( ! output ) + output = &output_; // use the local copy if the user hasn't provided their own + dds_sample sample_; + if( ! sample ) + sample = &sample_; // use the local copy if the user hasn't provided their own + auto status = reader->take_next_sample( &output->_raw, sample ); + if( status == ReturnCode_t::RETCODE_OK ) + { + // Only samples for which valid_data is true should be accessed + // valid_data indicates that the instance is still ALIVE and the `take` return an + // updated sample + if( ! sample->valid_data ) + output->invalidate(); + + return true; + } + if ( status == ReturnCode_t::RETCODE_NO_DATA ) + { + // This is an expected return code and is not an error + return false; + } + DDS_API_CALL_THROW( "list_parameters_request_msg::take_next", status ); +} + +} // namespace ros2 +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2-list-parameters-response-msg.cpp b/third-party/realdds/src/topics/ros2-list-parameters-response-msg.cpp new file mode 100644 index 0000000000..5bde58ed69 --- /dev/null +++ b/third-party/realdds/src/topics/ros2-list-parameters-response-msg.cpp @@ -0,0 +1,78 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + + +namespace realdds { +namespace topics { +namespace ros2 { + + +/*static*/ std::shared_ptr< dds_topic > +list_parameters_response_msg::create_topic( std::shared_ptr< dds_participant > const & participant, char const * topic_name ) +{ + return std::make_shared< dds_topic >( participant, + eprosima::fastdds::dds::TypeSupport( new list_parameters_response_msg::type ), + topic_name ); +} + + +/*static*/ bool +list_parameters_response_msg::take_next( dds_topic_reader & reader, list_parameters_response_msg * output, dds_sample * sample ) +{ + list_parameters_response_msg output_; + if( ! output ) + output = &output_; // use the local copy if the user hasn't provided their own + dds_sample sample_; + if( ! sample ) + sample = &sample_; // use the local copy if the user hasn't provided their own + auto status = reader->take_next_sample( &output->_raw, sample ); + if( status == ReturnCode_t::RETCODE_OK ) + { + // Only samples for which valid_data is true should be accessed + // valid_data indicates that the instance is still ALIVE and the `take` return an + // updated sample + if( ! sample->valid_data ) + output->clear(); + + return true; + } + if ( status == ReturnCode_t::RETCODE_NO_DATA ) + { + // This is an expected return code and is not an error + return false; + } + DDS_API_CALL_THROW( "list_parameters_response_msg::take_next", status ); +} + + +dds_sequence_number list_parameters_response_msg::write_to( dds_topic_writer & writer ) +{ + eprosima::fastrtps::rtps::WriteParams params; + bool success = DDS_API_CALL( writer.get()->write( &_raw, params ) ); + if( !success ) + { + LOG_ERROR( "Error writing message" ); + return 0; + } + // The params will contain, after the write, the sequence number (incremented automatically) for the sample that was + // sent. The source_timestamp is always INVALID for some reason. + return params.sample_identity().sequence_number().to64long(); +} + + +} // namespace ros2 +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2-set-parameters-request-msg.cpp b/third-party/realdds/src/topics/ros2-set-parameters-request-msg.cpp new file mode 100644 index 0000000000..78da78717c --- /dev/null +++ b/third-party/realdds/src/topics/ros2-set-parameters-request-msg.cpp @@ -0,0 +1,60 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#include +#include +#include + +#include +#include +#include + +#include +#include + + +namespace realdds { +namespace topics { +namespace ros2 { + + +/*static*/ std::shared_ptr< dds_topic > +set_parameters_request_msg::create_topic( std::shared_ptr< dds_participant > const & participant, char const * topic_name ) +{ + return std::make_shared< dds_topic >( participant, + eprosima::fastdds::dds::TypeSupport( new set_parameters_request_msg::type ), + topic_name ); +} + + +/*static*/ bool +set_parameters_request_msg::take_next( dds_topic_reader & reader, set_parameters_request_msg * output, dds_sample * sample ) +{ + set_parameters_request_msg output_; + if( ! output ) + output = &output_; // use the local copy if the user hasn't provided their own + dds_sample sample_; + if( ! sample ) + sample = &sample_; // use the local copy if the user hasn't provided their own + auto status = reader->take_next_sample( &output->_raw, sample ); + if( status == ReturnCode_t::RETCODE_OK ) + { + // Only samples for which valid_data is true should be accessed + // valid_data indicates that the instance is still ALIVE and the `take` return an + // updated sample + if( ! sample->valid_data ) + output->invalidate(); + + return true; + } + if ( status == ReturnCode_t::RETCODE_NO_DATA ) + { + // This is an expected return code and is not an error + return false; + } + DDS_API_CALL_THROW( "set_parameters_request_msg::take_next", status ); +} + +} // namespace ros2 +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2-set-parameters-response-msg.cpp b/third-party/realdds/src/topics/ros2-set-parameters-response-msg.cpp new file mode 100644 index 0000000000..a12e404ba9 --- /dev/null +++ b/third-party/realdds/src/topics/ros2-set-parameters-response-msg.cpp @@ -0,0 +1,78 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + + +namespace realdds { +namespace topics { +namespace ros2 { + + +/*static*/ std::shared_ptr< dds_topic > +set_parameters_response_msg::create_topic( std::shared_ptr< dds_participant > const & participant, char const * topic_name ) +{ + return std::make_shared< dds_topic >( participant, + eprosima::fastdds::dds::TypeSupport( new set_parameters_response_msg::type ), + topic_name ); +} + + +/*static*/ bool +set_parameters_response_msg::take_next( dds_topic_reader & reader, set_parameters_response_msg * output, dds_sample * sample ) +{ + set_parameters_response_msg output_; + if( ! output ) + output = &output_; // use the local copy if the user hasn't provided their own + dds_sample sample_; + if( ! sample ) + sample = &sample_; // use the local copy if the user hasn't provided their own + auto status = reader->take_next_sample( &output->_raw, sample ); + if( status == ReturnCode_t::RETCODE_OK ) + { + // Only samples for which valid_data is true should be accessed + // valid_data indicates that the instance is still ALIVE and the `take` return an + // updated sample + if( ! sample->valid_data ) + output->invalidate(); + + return true; + } + if ( status == ReturnCode_t::RETCODE_NO_DATA ) + { + // This is an expected return code and is not an error + return false; + } + DDS_API_CALL_THROW( "set_parameters_response_msg::take_next", status ); +} + + +dds_sequence_number set_parameters_response_msg::write_to( dds_topic_writer & writer ) +{ + eprosima::fastrtps::rtps::WriteParams params; + bool success = DDS_API_CALL( writer.get()->write( &_raw, params ) ); + if( ! success ) + { + LOG_ERROR( "Error writing message" ); + return 0; + } + // The params will contain, after the write, the sequence number (incremented automatically) for the sample that was + // sent. The source_timestamp is always INVALID for some reason. + return params.sample_identity().sequence_number().to64long(); +} + + +} // namespace ros2 +} // namespace topics +} // namespace realdds