diff --git a/third-party/realdds/include/realdds/dds-device-server.h b/third-party/realdds/include/realdds/dds-device-server.h index 8255bf14bc..b93dbc132f 100644 --- a/third-party/realdds/include/realdds/dds-device-server.h +++ b/third-party/realdds/include/realdds/dds-device-server.h @@ -123,6 +123,10 @@ class dds_device_server std::shared_ptr< dds_topic_reader > _control_reader; std::shared_ptr< dds_topic_writer > _metadata_writer; std::shared_ptr< dds_device_broadcaster > _broadcaster; + + std::shared_ptr< dds_topic_reader > _get_control_value_reader; + std::shared_ptr< dds_topic_writer > _get_control_value_writer; + dispatcher _control_dispatcher; set_option_callback _set_option_callback; diff --git a/third-party/realdds/include/realdds/topics/dds-topic-names.h b/third-party/realdds/include/realdds/topics/dds-topic-names.h index cb0d36e9ef..f83e8a4aed 100644 --- a/third-party/realdds/include/realdds/topics/dds-topic-names.h +++ b/third-party/realdds/include/realdds/topics/dds-topic-names.h @@ -24,6 +24,8 @@ constexpr char const * NOTIFICATION_TOPIC_NAME = "/notification"; constexpr char const * CONTROL_TOPIC_NAME = "/control"; constexpr char const * METADATA_TOPIC_NAME = "/metadata"; constexpr char const * DFU_TOPIC_NAME = "/dfu"; +constexpr char const * GET_CONTROL_VALUE_REQUEST_TOPIC = "rq/get_control_valueRequest"; +constexpr char const * GET_CONTROL_VALUE_RESPONSE_TOPIC = "rr/get_control_valueReply"; namespace notification { diff --git a/third-party/realdds/include/realdds/topics/getcontrolvalue-request-msg.h b/third-party/realdds/include/realdds/topics/getcontrolvalue-request-msg.h new file mode 100644 index 0000000000..42fbf1410b --- /dev/null +++ b/third-party/realdds/include/realdds/topics/getcontrolvalue-request-msg.h @@ -0,0 +1,49 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + +#pragma once + +#include +#include + +#include + +namespace realdds +{ + namespace topics + { + class GetControlValueRequestPubSubType; + } // namespace msg +} // namespace sensor_msgs + +namespace realdds +{ + + class dds_participant; + class dds_topic; + class dds_topic_reader; + + namespace topics + { + + class GetControlValueRequestMsg + { + public: + using type = realdds::topics::GetControlValueRequestPubSubType; + + GetControlValueRequestMsg() = default; + ~GetControlValueRequestMsg() = default; + + uint64_t const get_control_id() const { return _data.get_control_id(); } + void set_control_id(uint64_t new_control_id) { _data.set_control_id(new_control_id); } + + static std::shared_ptr create_topic(std::shared_ptr const &participant, + char const *topic_name); + + + private: + realdds::topics::GetControlValueRequest _data; + }; + + } // namespace topics +} // namespace realdds diff --git a/third-party/realdds/include/realdds/topics/getcontrolvalue-response-msg.h b/third-party/realdds/include/realdds/topics/getcontrolvalue-response-msg.h new file mode 100644 index 0000000000..f6e56ce367 --- /dev/null +++ b/third-party/realdds/include/realdds/topics/getcontrolvalue-response-msg.h @@ -0,0 +1,49 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + +#pragma once + +#include +#include + +#include + +namespace realdds +{ + namespace topics + { + class GetControlValueResponsePubSubType; + } // namespace msg +} // namespace sensor_msgs + +namespace realdds +{ + + class dds_participant; + class dds_topic; + class dds_topic_reader; + + namespace topics + { + + class GetControlValueResponseMsg + { + public: + using type = realdds::topics::GetControlValueResponsePubSubType; + + GetControlValueResponseMsg() = default; + ~GetControlValueResponseMsg() = default; + + uint64_t const get_result() const { return _data.get_result(); } + void set_result(uint64_t new_result) { _data.set_result(new_result); } + + static std::shared_ptr create_topic(std::shared_ptr const &participant, + char const *topic_name); + + + private: + realdds::topics::GetControlValueResponse _data; + }; + + } // namespace topics +} // namespace realdds diff --git a/third-party/realdds/include/realdds/topics/ros2/ros2getcontrolvalue-request.h b/third-party/realdds/include/realdds/topics/ros2/ros2getcontrolvalue-request.h new file mode 100644 index 0000000000..85a84832f0 --- /dev/null +++ b/third-party/realdds/include/realdds/topics/ros2/ros2getcontrolvalue-request.h @@ -0,0 +1,120 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + +#ifndef _GET_CONTROL_VALUE_REQSPONSE_H_ +#define _GET_CONTROL_VALUE_REQSPONSE_H_ + +#include +#include + +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec(dllexport) +#else +#define eProsima_user_DllExport +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define eProsima_user_DllExport +#endif // _WIN32 + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(GETCONTROLRESPONSE_SOURCE) +#define GETCONTROLRESPONSE_DllAPI __declspec(dllexport) +#else +#define GETCONTROLRESPONSE_DllAPI __declspec(dllimport) +#endif // +#else +#define GETCONTROLRESPONSE_DllAPI +#endif // +#else +#define GETCONTROLRESPONSE_DllAPI +#endif // _WIN32 + +namespace eprosima +{ + namespace fastcdr + { + class Cdr; + } // namespace fastcdr +} // namespace eprosima + +namespace realdds +{ + namespace topics + { + + class GetControlValueResponse + { + public: + // Default constructor + eProsima_user_DllExport GetControlValueResponse() = default; + + // Default constructor + eProsima_user_DllExport ~GetControlValueResponse() = default; + + eProsima_user_DllExport static size_t getMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function returns the serialized size of a data depending on the buffer alignment. + * @param data Data which is calculated its serialized size. + * @param current_alignment Buffer alignment. + * @return Serialized size. + */ + eProsima_user_DllExport static size_t getCdrSerializedSize( + const realdds::topics::GetControlValueResponse &data, + size_t current_alignment = 0); + + /*! + * @brief This function serializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr &cdr) const; + + /*! + * @brief This function deserializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr &cdr); + + /*! + * @brief This function returns the maximum serialized size of the Key of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function tells you if the Key has been defined for this type + */ + eProsima_user_DllExport static bool isKeyDefined(); + + /*! + * @brief This function serializes the key members of an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serializeKey( + eprosima::fastcdr::Cdr &cdr) const; + + eProsima_user_DllExport void set_result(long long result); + eProsima_user_DllExport long long get_result() const; + + private: + long long result; + }; + + } // namespace topics +} // namespace realdds + +#endif diff --git a/third-party/realdds/include/realdds/topics/ros2/ros2getcontrolvalue-requestPubSubTypes.h b/third-party/realdds/include/realdds/topics/ros2/ros2getcontrolvalue-requestPubSubTypes.h new file mode 100644 index 0000000000..6d506811ad --- /dev/null +++ b/third-party/realdds/include/realdds/topics/ros2/ros2getcontrolvalue-requestPubSubTypes.h @@ -0,0 +1,70 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + + +#ifndef _GET_CONTROL_VALUE_REQUEST_PUBSUBTYPES_H_ +#define _GET_CONTROL_VALUE_REQUEST_PUBSUBTYPES_H_ + +#include +#include +#include "ros2getcontrolvalue-request.h" + +#if !defined(GEN_API_VER) || (GEN_API_VER != 1) +#error Generated Header is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen. +#endif // GEN_API_VER + +namespace realdds +{ + namespace topics + { + + class GetControlValueRequestPubSubType : public eprosima::fastdds::dds::TopicDataType + { + public: + typedef GetControlValueRequest type; + + eProsima_user_DllExport GetControlValueRequestPubSubType(); + eProsima_user_DllExport virtual ~GetControlValueRequestPubSubType() override; + + eProsima_user_DllExport bool serialize(void *data, eprosima::fastrtps::rtps::SerializedPayload_t *payload) override; + eProsima_user_DllExport bool deserialize(eprosima::fastrtps::rtps::SerializedPayload_t *payload, void *data) override; + eProsima_user_DllExport std::function getSerializedSizeProvider(void *data) override; + eProsima_user_DllExport void *createData() override; + eProsima_user_DllExport void deleteData(void *data) override; + eProsima_user_DllExport bool getKey(void *data, eprosima::fastrtps::rtps::InstanceHandle_t *handle, bool force_md5 = false) override; + +#ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + eProsima_user_DllExport inline bool is_bounded() const override + { + return false; + } + +#endif // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + +#ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + eProsima_user_DllExport inline bool is_plain() const override + { + return false; + } + +#endif // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + +#ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + eProsima_user_DllExport inline bool construct_sample( + void *memory) const override + { + (void)memory; + return false; + } + +#endif // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + + private: + MD5 m_md5; + unsigned char *m_keyBuffer; + }; + + } // namespace topics +} // namespace realdds + +#endif diff --git a/third-party/realdds/include/realdds/topics/ros2/ros2getcontrolvalue-response.h b/third-party/realdds/include/realdds/topics/ros2/ros2getcontrolvalue-response.h new file mode 100644 index 0000000000..eaca9aff1d --- /dev/null +++ b/third-party/realdds/include/realdds/topics/ros2/ros2getcontrolvalue-response.h @@ -0,0 +1,120 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + +#ifndef _GET_CONTROL_VALUE_RESPONSE_H_ +#define _GET_CONTROL_VALUE_RESPONSE_H_ + +#include +#include + +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec(dllexport) +#else +#define eProsima_user_DllExport +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define eProsima_user_DllExport +#endif // _WIN32 + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(GETCONTROLRESPONSE_SOURCE) +#define GETCONTROLRESPONSE_DllAPI __declspec(dllexport) +#else +#define GETCONTROLRESPONSE__DllAPI __declspec(dllimport) +#endif // +#else +#define GETCONTROLRESPONSE__DllAPI +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define GETCONTROLRESPONSE__DllAPI +#endif // _WIN32 + +namespace eprosima +{ + namespace fastcdr + { + class Cdr; + } // namespace fastcdr +} // namespace eprosima + +namespace realdds +{ + namespace topics + { + + class GetControlValueResponse + { + public: + // Default constructor + eProsima_user_DllExport GetControlValueResponse() = default; + + // Default constructor + eProsima_user_DllExport ~GetControlValueResponse() = default; + + eProsima_user_DllExport static size_t getMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function returns the serialized size of a data depending on the buffer alignment. + * @param data Data which is calculated its serialized size. + * @param current_alignment Buffer alignment. + * @return Serialized size. + */ + eProsima_user_DllExport static size_t getCdrSerializedSize( + const realdds::topics::GetControlValueResponse &data, + size_t current_alignment = 0); + + /*! + * @brief This function serializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr &cdr) const; + + /*! + * @brief This function deserializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr &cdr); + + /*! + * @brief This function returns the maximum serialized size of the Key of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function tells you if the Key has been defined for this type + */ + eProsima_user_DllExport static bool isKeyDefined(); + + /*! + * @brief This function serializes the key members of an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serializeKey( + eprosima::fastcdr::Cdr &cdr) const; + + eProsima_user_DllExport void set_result(int64_t id); + eProsima_user_DllExport long long get_result() const; + + private: + long long result; + }; + + } // namespace topics +} // namespace realdds + +#endif diff --git a/third-party/realdds/include/realdds/topics/ros2/ros2getcontrolvalue-responsePubSubTypes.h b/third-party/realdds/include/realdds/topics/ros2/ros2getcontrolvalue-responsePubSubTypes.h new file mode 100644 index 0000000000..06c8bba570 --- /dev/null +++ b/third-party/realdds/include/realdds/topics/ros2/ros2getcontrolvalue-responsePubSubTypes.h @@ -0,0 +1,70 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + + +#ifndef _GET_CONTROL_VALUE_RESPONSE_PUBSUBTYPES_H_ +#define _GET_CONTROL_VALUE_RESPONSE_PUBSUBTYPES_H_ + +#include +#include +#include "ros2getcontrolvalue-response.h" + +#if !defined(GEN_API_VER) || (GEN_API_VER != 1) +#error Generated Header is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen. +#endif // GEN_API_VER + +namespace realdds +{ + namespace topics + { + + class GetControlValueResponsePubSubType : public eprosima::fastdds::dds::TopicDataType + { + public: + typedef GetControlValueResponse type; + + eProsima_user_DllExport GetControlValueResponsePubSubType(); + eProsima_user_DllExport virtual ~GetControlValueResponsePubSubType() override; + + eProsima_user_DllExport bool serialize(void *data, eprosima::fastrtps::rtps::SerializedPayload_t *payload) override; + eProsima_user_DllExport bool deserialize(eprosima::fastrtps::rtps::SerializedPayload_t *payload, void *data) override; + eProsima_user_DllExport std::function getSerializedSizeProvider(void *data) override; + eProsima_user_DllExport void *createData() override; + eProsima_user_DllExport void deleteData(void *data) override; + eProsima_user_DllExport bool getKey(void *data, eprosima::fastrtps::rtps::InstanceHandle_t *handle, bool force_md5 = false) override; + +#ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + eProsima_user_DllExport inline bool is_bounded() const override + { + return false; + } + +#endif // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + +#ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + eProsima_user_DllExport inline bool is_plain() const override + { + return false; + } + +#endif // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + +#ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + eProsima_user_DllExport inline bool construct_sample( + void *memory) const override + { + (void)memory; + return false; + } + +#endif // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + + private: + MD5 m_md5; + unsigned char *m_keyBuffer; + }; + + } // namespace topics +} // namespace realdds + +#endif diff --git a/third-party/realdds/src/dds-device-server.cpp b/third-party/realdds/src/dds-device-server.cpp index 159ccf4f80..9a7a9aabcf 100644 --- a/third-party/realdds/src/dds-device-server.cpp +++ b/third-party/realdds/src/dds-device-server.cpp @@ -20,6 +20,9 @@ #include #include #include +#include +#include +#include #include #include @@ -238,6 +241,23 @@ void dds_device_server::init( std::vector< std::shared_ptr< dds_stream_server > rqos.override_from_json( _subscriber->get_participant()->settings().nested( "device", "control" ) ); _control_reader->run( rqos ); } + + if (auto topic = topics::GetControlValueRequestMsg::create_topic(_subscriber->get_participant(), topics::GET_CONTROL_VALUE_REQUEST_TOPIC)) + { + _get_control_value_reader = std::make_shared(topic, _subscriber); + _get_control_value_reader->on_data_available([&]() + { std::cout << " Samer reader" << std::endl; }); + realdds::dds_topic_reader::qos rqos(eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS); + _get_control_value_reader->run(rqos); + } + + if (auto topic = topics::GetControlValueRequestMsg::create_topic(_publisher->get_participant(), topics::GET_CONTROL_VALUE_RESPONSE_TOPIC)) + { + _get_control_value_writer = std::make_shared(topic, _publisher); + realdds::dds_topic_writer::qos wqos(eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS); + wqos.history().depth = 10; // default is 1 + _get_control_value_writer->run(wqos); + } } catch( std::exception const & ) { diff --git a/third-party/realdds/src/topics/getcontrolvalue-request-msg.cpp b/third-party/realdds/src/topics/getcontrolvalue-request-msg.cpp new file mode 100644 index 0000000000..feb2fa66be --- /dev/null +++ b/third-party/realdds/src/topics/getcontrolvalue-request-msg.cpp @@ -0,0 +1,25 @@ +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace realdds +{ + namespace topics + { + + /*static*/ std::shared_ptr + GetControlValueRequestMsg::create_topic(std::shared_ptr const &participant, char const *topic_name) + { + return std::make_shared(participant, + eprosima::fastdds::dds::TypeSupport(new GetControlValueRequestMsg::type), + topic_name); + } + } // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/getcontrolvalue-response-msg.cpp b/third-party/realdds/src/topics/getcontrolvalue-response-msg.cpp new file mode 100644 index 0000000000..dc3a112d99 --- /dev/null +++ b/third-party/realdds/src/topics/getcontrolvalue-response-msg.cpp @@ -0,0 +1,25 @@ +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace realdds +{ + namespace topics + { + + /*static*/ std::shared_ptr + GetControlValueResponseMsg::create_topic(std::shared_ptr const &participant, char const *topic_name) + { + return std::make_shared(participant, + eprosima::fastdds::dds::TypeSupport(new GetControlValueResponseMsg::type), + topic_name); + } + } // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-request.cpp b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-request.cpp new file mode 100644 index 0000000000..334fa2f3a9 --- /dev/null +++ b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-request.cpp @@ -0,0 +1,76 @@ +// File: ros2getcontrolvalue.cpp + +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + +#include "realdds/topics/ros2/ros2getcontrolvalue-request.h" +#include "ros2getcontrolvalue-requestTypeObject.h" +#include + +#include +using namespace eprosima::fastcdr::exception; + +#include + +namespace realdds +{ + namespace topics + { + + size_t GetControlValueRequest::getMaxCdrSerializedSize( + size_t current_alignment) + { + size_t initial_alignment = current_alignment; + + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + 255 + 1; + + return current_alignment - initial_alignment; + } + + size_t GetControlValueRequest::getCdrSerializedSize( + const GetControlValueRequest &data, + size_t current_alignment) + { + (void)data; + size_t initial_alignment = current_alignment; + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + sizeof(data.control_id) + 1; + return current_alignment - initial_alignment; + } + + void GetControlValueRequest::serialize( + eprosima::fastcdr::Cdr &scdr) const + { + + scdr << control_id; + } + + void GetControlValueRequest::deserialize( + eprosima::fastcdr::Cdr &dcdr) + { + dcdr >> control_id; + } + + size_t GetControlValueRequest::getKeyMaxCdrSerializedSize( + size_t current_alignment) + { + size_t current_align = current_alignment; + + return current_align; + } + + bool GetControlValueRequest::isKeyDefined() + { + return false; + } + + void GetControlValueRequest::set_control_id(int64_t id) + { + control_id = id; + } + int64_t GetControlValueRequest::get_control_id() const + { + return control_id; + } + + } // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-requestPubSubTypes.cpp b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-requestPubSubTypes.cpp new file mode 100644 index 0000000000..39ca13e2de --- /dev/null +++ b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-requestPubSubTypes.cpp @@ -0,0 +1,106 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + +#include +#include + +#include "realdds/topics/ros2/ros2getcontrolvalue-requsetPubSubTypes.h" + +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t; + + +namespace realdds { +namespace topics { + +GetControlValueRequestPubSubType::GetControlValueRequestPubSubType() +{ + setName("realdds:topics::dds_::GetControlValueRequest_"); + m_typeSize = 4 + 8; // Encapsulation + long field size + m_isGetKeyDefined = false; + m_keyBuffer = nullptr; +} + +GetControlValueRequestPubSubType::~GetControlValueRequestPubSubType() +{ + if (m_keyBuffer != nullptr) + { + free(m_keyBuffer); + } +} + +bool GetControlValueRequestPubSubType::serialize(void* data, eprosima::fastrtps::rtps::SerializedPayload_t* payload) +{ + GetControlValueRequest* p_type = static_cast(data); + + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->max_size); + eprosima::fastcdr::Cdr ser(fastbuffer); + + payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + ser.serialize_encapsulation(); + + try + { + ser << p_type->get_control_id(); + } + catch (eprosima::fastcdr::exception::NotEnoughMemoryException&) + { + return false; + } + + payload->length = static_cast(ser.getSerializedDataLength()); + return true; +} + +bool GetControlValueRequestPubSubType::deserialize(eprosima::fastrtps::rtps::SerializedPayload_t* payload, void* data) +{ + try + { + + GetControlValueRequest *p_type = static_cast(data); + + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->length); + eprosima::fastcdr::Cdr deser(fastbuffer); + + deser.read_encapsulation(); + payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + // Deserialize the object. + p_type->deserialize(deser); + } + catch (eprosima::fastcdr::exception::NotEnoughMemoryException &) + { + return false; + } + + return true; +} + +std::function GetControlValueRequestPubSubType::getSerializedSizeProvider(void* data) +{ + return [data]() -> uint32_t + { + return static_cast(8 + 4); // long field + encapsulation + }; +} + +void* GetControlValueRequestPubSubType::createData() +{ + return new GetControlValueRequest(); +} + +void GetControlValueRequestPubSubType::deleteData(void* data) +{ + delete static_cast(data); +} + +bool GetControlValueRequestPubSubType::getKey(void* data, eprosima::fastrtps::rtps::InstanceHandle_t* handle, bool force_md5) +{ + (void)data; + (void)handle; + (void)force_md5; + return false; +} + +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-requestTypeObject.cpp b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-requestTypeObject.cpp new file mode 100644 index 0000000000..ddeeb7534b --- /dev/null +++ b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-requestTypeObject.cpp @@ -0,0 +1,15 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + +#include "ros2getcontrolvalue-requestTypeObject.h" + +namespace realdds { +namespace topics { + +void registerGetControlValueRequestTypes() +{ + // Registration logic for DDS TypeObject if needed. +} + +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-requestTypeObject.h b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-requestTypeObject.h new file mode 100644 index 0000000000..a2303536f8 --- /dev/null +++ b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-requestTypeObject.h @@ -0,0 +1,46 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + +#ifndef _GET_CONTROL_VALUE_REQUEST_TYPE_OBJECT_H_ +#define _GET_CONTROL_VALUE_REQUEST_TYPE_OBJECT_H_ + +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif +#else +#define eProsima_user_DllExport +#endif + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(GETCONTROLREQUEST_SOURCE) +#define GETCONTROLREQUEST_DllAPI __declspec( dllexport ) +#else +#define GETCONTROLREQUEST_DllAPI __declspec( dllimport ) +#endif // GETCONTROLREQUEST_SOURCE +#else +#define GETCONTROLREQUEST_DllAPI +#endif +#else +#define GETCONTROLREQUEST_DllAPI +#endif // _WIN32 + +using namespace eprosima::fastrtps::types; + +eProsima_user_DllExport void registerHeaderTypes(); + + +namespace realdds { +namespace topics { + +eProsima_user_DllExport void registerGetControlValueRequestTypes(); + +} // namespace topics +} // namespace realdds + +#endif \ No newline at end of file diff --git a/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-response.cpp b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-response.cpp new file mode 100644 index 0000000000..6e12685ee8 --- /dev/null +++ b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-response.cpp @@ -0,0 +1,76 @@ +// File: ros2getcontrolvalue.cpp + +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + +#include "realdds/topics/ros2/ros2getcontrolvalue-response.h" +#include "ros2getcontrolvalue-responseTypeObject.h" +#include + +#include +using namespace eprosima::fastcdr::exception; + +#include + +namespace realdds +{ + namespace topics + { + + size_t GetControlValueResponse::getMaxCdrSerializedSize( + size_t current_alignment) + { + size_t initial_alignment = current_alignment; + + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + 255 + 1; + + return current_alignment - initial_alignment; + } + + size_t GetControlValueResponse::getCdrSerializedSize( + const GetControlValueResponse &data, + size_t current_alignment) + { + (void)data; + size_t initial_alignment = current_alignment; + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4) + sizeof(data.result) + 1; + return current_alignment - initial_alignment; + } + + void GetControlValueResponse::serialize( + eprosima::fastcdr::Cdr &scdr) const + { + + scdr << result; + } + + void GetControlValueResponse::deserialize( + eprosima::fastcdr::Cdr &dcdr) + { + dcdr >> result; + } + + size_t GetControlValueResponse::getKeyMaxCdrSerializedSize( + size_t current_alignment) + { + size_t current_align = current_alignment; + + return current_align; + } + + bool GetControlValueResponse::isKeyDefined() + { + return false; + } + + void GetControlValueResponse::set_result(long long new_result) + { + result = new_result; + } + long long GetControlValueResponse::get_result() const + { + return result; + } + + } // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-responsePubSubTypes.cpp b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-responsePubSubTypes.cpp new file mode 100644 index 0000000000..39ca13e2de --- /dev/null +++ b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-responsePubSubTypes.cpp @@ -0,0 +1,106 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + +#include +#include + +#include "realdds/topics/ros2/ros2getcontrolvalue-requsetPubSubTypes.h" + +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t; + + +namespace realdds { +namespace topics { + +GetControlValueRequestPubSubType::GetControlValueRequestPubSubType() +{ + setName("realdds:topics::dds_::GetControlValueRequest_"); + m_typeSize = 4 + 8; // Encapsulation + long field size + m_isGetKeyDefined = false; + m_keyBuffer = nullptr; +} + +GetControlValueRequestPubSubType::~GetControlValueRequestPubSubType() +{ + if (m_keyBuffer != nullptr) + { + free(m_keyBuffer); + } +} + +bool GetControlValueRequestPubSubType::serialize(void* data, eprosima::fastrtps::rtps::SerializedPayload_t* payload) +{ + GetControlValueRequest* p_type = static_cast(data); + + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->max_size); + eprosima::fastcdr::Cdr ser(fastbuffer); + + payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + ser.serialize_encapsulation(); + + try + { + ser << p_type->get_control_id(); + } + catch (eprosima::fastcdr::exception::NotEnoughMemoryException&) + { + return false; + } + + payload->length = static_cast(ser.getSerializedDataLength()); + return true; +} + +bool GetControlValueRequestPubSubType::deserialize(eprosima::fastrtps::rtps::SerializedPayload_t* payload, void* data) +{ + try + { + + GetControlValueRequest *p_type = static_cast(data); + + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->length); + eprosima::fastcdr::Cdr deser(fastbuffer); + + deser.read_encapsulation(); + payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + // Deserialize the object. + p_type->deserialize(deser); + } + catch (eprosima::fastcdr::exception::NotEnoughMemoryException &) + { + return false; + } + + return true; +} + +std::function GetControlValueRequestPubSubType::getSerializedSizeProvider(void* data) +{ + return [data]() -> uint32_t + { + return static_cast(8 + 4); // long field + encapsulation + }; +} + +void* GetControlValueRequestPubSubType::createData() +{ + return new GetControlValueRequest(); +} + +void GetControlValueRequestPubSubType::deleteData(void* data) +{ + delete static_cast(data); +} + +bool GetControlValueRequestPubSubType::getKey(void* data, eprosima::fastrtps::rtps::InstanceHandle_t* handle, bool force_md5) +{ + (void)data; + (void)handle; + (void)force_md5; + return false; +} + +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-responseTypeObject.cpp b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-responseTypeObject.cpp new file mode 100644 index 0000000000..ddeeb7534b --- /dev/null +++ b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-responseTypeObject.cpp @@ -0,0 +1,15 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + +#include "ros2getcontrolvalue-requestTypeObject.h" + +namespace realdds { +namespace topics { + +void registerGetControlValueRequestTypes() +{ + // Registration logic for DDS TypeObject if needed. +} + +} // namespace topics +} // namespace realdds diff --git a/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-responseTypeObject.h b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-responseTypeObject.h new file mode 100644 index 0000000000..a2303536f8 --- /dev/null +++ b/third-party/realdds/src/topics/ros2/ros2getcontrolvalue-responseTypeObject.h @@ -0,0 +1,46 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. + +#ifndef _GET_CONTROL_VALUE_REQUEST_TYPE_OBJECT_H_ +#define _GET_CONTROL_VALUE_REQUEST_TYPE_OBJECT_H_ + +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif +#else +#define eProsima_user_DllExport +#endif + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(GETCONTROLREQUEST_SOURCE) +#define GETCONTROLREQUEST_DllAPI __declspec( dllexport ) +#else +#define GETCONTROLREQUEST_DllAPI __declspec( dllimport ) +#endif // GETCONTROLREQUEST_SOURCE +#else +#define GETCONTROLREQUEST_DllAPI +#endif +#else +#define GETCONTROLREQUEST_DllAPI +#endif // _WIN32 + +using namespace eprosima::fastrtps::types; + +eProsima_user_DllExport void registerHeaderTypes(); + + +namespace realdds { +namespace topics { + +eProsima_user_DllExport void registerGetControlValueRequestTypes(); + +} // namespace topics +} // namespace realdds + +#endif \ No newline at end of file