diff --git a/src/modules/uxrce_dds_client/CMakeLists.txt b/src/modules/uxrce_dds_client/CMakeLists.txt index 2150f9456f..10beaf69be 100644 --- a/src/modules/uxrce_dds_client/CMakeLists.txt +++ b/src/modules/uxrce_dds_client/CMakeLists.txt @@ -142,6 +142,10 @@ else() ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h uxrce_dds_client.cpp uxrce_dds_client.h + vehicle_command_srv.cpp + vehicle_command_srv.h + srv_base.cpp + srv_base.h DEPENDS microxrceddsclient libmicroxrceddsclient diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 8b62f01cdb..b09de9cc70 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -79,9 +79,6 @@ subscriptions: - topic: /fmu/in/vehicle_visual_odometry type: px4_msgs::msg::VehicleOdometry - - topic: /fmu/in/vehicle_command - type: px4_msgs::msg::VehicleCommand - - topic: /fmu/in/vehicle_trajectory_bezier type: px4_msgs::msg::VehicleTrajectoryBezier diff --git a/src/modules/uxrce_dds_client/generate_dds_topics.py b/src/modules/uxrce_dds_client/generate_dds_topics.py index ff31bbbedc..108b8b0c79 100644 --- a/src/modules/uxrce_dds_client/generate_dds_topics.py +++ b/src/modules/uxrce_dds_client/generate_dds_topics.py @@ -2,7 +2,7 @@ ################################################################################ # # Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). -# Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2018-2023 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: diff --git a/src/modules/uxrce_dds_client/srv_base.cpp b/src/modules/uxrce_dds_client/srv_base.cpp new file mode 100644 index 0000000000..679387a41d --- /dev/null +++ b/src/modules/uxrce_dds_client/srv_base.cpp @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "srv_base.h" + +#include +//#include "utilities.hpp" + + +#define TOPIC_NAME_SIZE 128 +#define REQUEST_TYPE_SIZE 128 +#define REPLY_TYPE_SIZE 128 + +static bool generate_request_name(char *request, const char *client_namespace, const char *name) +{ + if (client_namespace != nullptr) { + int ret = snprintf(request, TOPIC_NAME_SIZE, "rq/%s/fmu/%sRequest", client_namespace, name); + return (ret > 0 && ret < TOPIC_NAME_SIZE); + } + + int ret = snprintf(request, TOPIC_NAME_SIZE, "rq/fmu/%sRequest", name); + return (ret > 0 && ret < TOPIC_NAME_SIZE); +} + +static bool generate_reply_name(char *reply, const char *client_namespace, const char *name) +{ + if (client_namespace != nullptr) { + int ret = snprintf(reply, TOPIC_NAME_SIZE, "rr/%s/fmu/%sReply", client_namespace, name); + return (ret > 0 && ret < TOPIC_NAME_SIZE); + } + + int ret = snprintf(reply, TOPIC_NAME_SIZE, "rr/fmu/%sReply", name); + return (ret > 0 && ret < TOPIC_NAME_SIZE); +} + +static bool generate_request_type_name(char *request, const char *name) +{ + int ret = snprintf(request, REQUEST_TYPE_SIZE, "px4_msgs::srv::dds_::%s_Request_", name); + return (ret > 0 && ret < REQUEST_TYPE_SIZE); +} + +static bool generate_reply_type_name(char *reply, const char *name) +{ + int ret = snprintf(reply, REPLY_TYPE_SIZE, "px4_msgs::srv::dds_::%s_Response_", name); + return (ret > 0 && ret < REPLY_TYPE_SIZE); +} + +SrvBase::SrvBase(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId input_stream_id, + uxrObjectId participant_id) : + session_(session), + reliable_out_stream_id_(reliable_out_stream_id) + +{ + +} + +bool SrvBase::create_replier(uxrStreamId input_stream_id, + uxrObjectId participant_id, uint16_t index, const char *client_namespace, const char *service_name_simple, + const char *service_type_name_simple, uint16_t queue_depth) +{ +// request and reply names + char request_name[TOPIC_NAME_SIZE]; + char reply_name[TOPIC_NAME_SIZE]; + + if (!generate_request_name(request_name, client_namespace, service_name_simple)) { + return false; + } + + if (!generate_reply_name(reply_name, client_namespace, service_name_simple)) { + return false; + } + + // request and reply types + char request_type_name[REQUEST_TYPE_SIZE]; + char reply_type_name[REPLY_TYPE_SIZE]; + + if (!generate_request_type_name(request_type_name, service_type_name_simple)) { + return false; + } + + if (!generate_reply_type_name(reply_type_name, service_type_name_simple)) { + return false; + } + + + // Use the second half of the available ID space. + // Add 1 so that we get a nice hex starting number: 0x800 instead of 0x7ff. + uint16_t id = index + (65535U / 32U) + 1; + + replier_id_ = uxr_object_id(id, UXR_REPLIER_ID); + + //char service_name[TOPIC_NAME_SIZE]; + + const uxrQoS_t qos = { + .durability = UXR_DURABILITY_PERSISTENT, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 1, + }; + + uint16_t replier_req = uxr_buffer_create_replier_bin(session_, reliable_out_stream_id_, replier_id_, participant_id, + service_name_simple, request_type_name, reply_type_name, request_name, reply_name, qos, UXR_REPLACE); + uint8_t status; + + if (!uxr_run_session_until_all_status(session_, 1000, &replier_req, &status, 1)) { + return false; + } + + + // Request requests + uxrDeliveryControl delivery_control = { + 0 + }; + delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED; + uint16_t read_data_req = + uxr_buffer_request_data(session_, reliable_out_stream_id_, replier_id_, input_stream_id, &delivery_control); + (void) read_data_req; + + return true; +} diff --git a/src/modules/uxrce_dds_client/srv_base.h b/src/modules/uxrce_dds_client/srv_base.h new file mode 100644 index 0000000000..d8a038d42b --- /dev/null +++ b/src/modules/uxrce_dds_client/srv_base.h @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +/** @class SrvBase The SrvBase class defines the common properties and methods of a service requester. */ +class SrvBase +{ +public: + /** + * @brief Constructor. + * @param session pointer to the micro xrce-dds session. + * @param reliable_out_stream_id output stream ID. + * @param input_stream_id input stream ID. + * @param participant_id participant ID. + * @return Returns false iff successful, otherwise false. + */ + SrvBase(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId input_stream_id, + uxrObjectId participant_id); + + virtual ~SrvBase() + { + + }; + + /** + * @brief Virtual method that process an incoming request from xrce_dds. + * @param ub Buffer that stores the incoming request message. + * @param time_offset_us time offset between agent and client. + * @return Returns false iff successful, otherwise false. + */ + virtual bool process_request(ucdrBuffer *ub, const int64_t time_offset_us) = 0; + + /** + * @brief Virtual method that process and send a reply. + * @return Returns false iff successful, otherwise false. + */ + virtual bool process_reply() = 0; + + /** @var is_reply_pending_ Flag for pending replies */ + bool is_reply_pending_; + + /** @var session_ xrce_dds session pointer */ + uxrSession *session_; + + /** @var reliable_out_stream_id_ output stream */ + uxrStreamId reliable_out_stream_id_; + + /** @var replier_id_ uxrce_dds replier */ + uxrObjectId replier_id_; + + /** @var sample_id_ uxrce_dds sample identifier to link request and reply */ + SampleIdentity sample_id_; + +protected: + /** + * @brief xrce_dds replier creator. + * @param input_stream_id input stream. + * @param participant_id partecipant id. + * @param index index used to create the replier id. + * @param client_namespace namespace of the client. + * @param service_name_simple name of the service. + * @param service_type_name_simple name of the service type. + * @param queue_depth lenght of the queue. + * @return Returns false iff successful, otherwise false. + */ + bool create_replier(uxrStreamId input_stream_id, uxrObjectId participant_id, uint16_t index, + const char *client_namespace, const char *service_name_simple, const char *service_type_name_simple, + uint16_t queue_depth); +}; diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index c2274bf915..4890582464 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +37,9 @@ #include "uxrce_dds_client.h" +// services +#include "vehicle_command_srv.h" + #include #include #include @@ -73,8 +76,6 @@ void on_time(uxrSession *session, int64_t current_time, int64_t received_timesta Timesync *timesync = static_cast(args); timesync->update(current_time / 1000, transmit_timestamp, originate_timestamp); - //fprintf(stderr, "time_offset: %ld, timesync: %ld, diff: %ld\n", session->time_offset/1000, timesync->offset(), session->time_offset/1000 + timesync->offset()); - session->time_offset = -timesync->offset() * 1000; // us -> ns } } @@ -85,6 +86,27 @@ void on_time_no_sync(uxrSession *session, int64_t current_time, int64_t received session->time_offset = 0; } + +void on_request( + uxrSession *session, + uxrObjectId object_id, + uint16_t request_id, + SampleIdentity *sample_id, + ucdrBuffer *ub, + uint16_t length, + void *args) +{ + (void) request_id; + (void) length; + (void) args; + + const int64_t time_offset_us = session->time_offset / 1000; // ns -> us + + UxrceddsClient *client = (UxrceddsClient *)args; + + client->process_requests(object_id, sample_id, ub, time_offset_us); +} + UxrceddsClient::UxrceddsClient(Transport transport, const char *device, int baudrate, const char *agent_ip, const char *port, bool localhost_only, bool custom_participant, const char *client_namespace, bool synchronize_timestamps) : @@ -153,6 +175,8 @@ UxrceddsClient::~UxrceddsClient() delete _subs; delete _pubs; + delete_repliers(); + if (_transport_serial) { uxr_close_serial_transport(_transport_serial); delete _transport_serial; @@ -225,10 +249,8 @@ void UxrceddsClient::run() uxrStreamId reliable_in = uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer, sizeof(input_reliable_stream_buffer), STREAM_HISTORY); - (void)reliable_in; uxrStreamId best_effort_in = uxr_create_input_best_effort_stream(&session); - (void)best_effort_in; // Create entities uxrObjectId participant_id = uxr_object_id(0x01, UXR_PARTICIPANT_ID); @@ -300,6 +322,15 @@ void UxrceddsClient::run() return; } + // create VehicleCommand replier + if (num_of_repliers < MAX_NUM_REPLIERS) { + if (add_replier(new VehicleCommandSrv(&session, reliable_out, reliable_in, participant_id, _client_namespace, + num_of_repliers))) { + PX4_ERR("replier init failed"); + return; + } + } + _connected = true; // Set time-callback. @@ -310,6 +341,8 @@ void UxrceddsClient::run() uxr_set_time_callback(&session, on_time_no_sync, nullptr); } + uxr_set_request_callback(&session, on_request, this); + // Synchronize with the Agent bool synchronized = false; @@ -359,6 +392,9 @@ void UxrceddsClient::run() _subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace); + // check if there are available replies + process_replies(); + uxr_run_session_timeout(&session, 0); // time sync session @@ -409,6 +445,8 @@ void UxrceddsClient::run() } + delete_repliers(); + uxr_delete_session_retries(&session, _connected ? 1 : 0); _last_payload_tx_rate = 0; _last_payload_tx_rate = 0; @@ -553,6 +591,46 @@ int UxrceddsClient::setBaudrate(int fd, unsigned baud) return 0; } +bool UxrceddsClient::add_replier(SrvBase *replier) +{ + if (num_of_repliers < MAX_NUM_REPLIERS) { + repliers_[num_of_repliers] = replier; + + num_of_repliers++; + } + + return false; +} + +void UxrceddsClient::process_requests(uxrObjectId object_id, SampleIdentity *sample_id, ucdrBuffer *ub, + const int64_t time_offset_us) +{ + for (uint8_t i = 0; i < num_of_repliers; i++) { + if (object_id.id == repliers_[i]->replier_id_.id + && object_id.type == repliers_[i]->replier_id_.type) { + repliers_[i]->process_request(ub, time_offset_us); + memcpy(&(repliers_[i]->sample_id_), sample_id, sizeof(repliers_[i]->sample_id_)); + break; + } + } +} + +void UxrceddsClient::process_replies() +{ + for (uint8_t i = 0; i < num_of_repliers; i++) { + repliers_[i]->process_reply(); + } +} + +void UxrceddsClient::delete_repliers() +{ + for (uint8_t i = 0; i < num_of_repliers; i++) { + delete (repliers_[i]); + } + + num_of_repliers = 0; +} + int UxrceddsClient::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h index f757040214..55f6dfb9e4 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,6 +40,10 @@ #include +#include "srv_base.h" + +#define MAX_NUM_REPLIERS 5 + class UxrceddsClient : public ModuleBase, public ModuleParams { public: @@ -71,6 +75,34 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; + /** + * @brief Method to add a new replyer to the replier array. + * @param replier pointer to the new replier. + * @return Returns false iff successful, otherwise false. + */ + bool add_replier(SrvBase *replier); + + /** + * @brief Method to process new incoming requests and dispatch them to the appropriate server. + * @param object_id replier object id + * @param sample_id pointer to specific request. + * @param time_offset_us time offset between agent and client. + * @param ub pointer to the received request data + */ + void process_requests(uxrObjectId object_id, SampleIdentity *sample_id, ucdrBuffer *ub, const int64_t time_offset_us); + + /** + * @brief Method to process the available replies. + * @return Returns false iff successful, otherwise false. + */ + void process_replies(); + + /** + * @brief Method to delete all repliers. + * @return Returns false iff successful, otherwise false. + */ + void delete_repliers(); + private: int setBaudrate(int fd, unsigned baud); @@ -79,7 +111,6 @@ private: const char *_client_namespace; const bool _synchronize_timestamps; - // max port characters (5+'\0') static const uint8_t PORT_MAX_LENGTH = 6; // max agent ip characters (15+'\0') @@ -93,6 +124,9 @@ private: SendTopicsSubs *_subs{nullptr}; RcvTopicsPubs *_pubs{nullptr}; + SrvBase *repliers_[MAX_NUM_REPLIERS]; + uint8_t num_of_repliers{0}; + uxrSerialTransport *_transport_serial{nullptr}; uxrUDPTransport *_transport_udp{nullptr}; uxrCommunication *_comm{nullptr}; diff --git a/src/modules/uxrce_dds_client/vehicle_command_srv.cpp b/src/modules/uxrce_dds_client/vehicle_command_srv.cpp new file mode 100644 index 0000000000..aac0bd2c58 --- /dev/null +++ b/src/modules/uxrce_dds_client/vehicle_command_srv.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "vehicle_command_srv.h" +#include +#include + +VehicleCommandSrv::VehicleCommandSrv(uxrSession *session, uxrStreamId reliable_out_stream_id, + uxrStreamId input_stream_id, uxrObjectId participant_id, const char *client_namespace, const uint8_t index) : + SrvBase(session, reliable_out_stream_id, input_stream_id, participant_id) +{ + uint16_t queue_depth = uORB::DefaultQueueSize::value * + 2; // use a bit larger queue size than internal + create_replier(input_stream_id, participant_id, index, client_namespace, "vehicle_command", "VehicleCommand", + queue_depth); +}; + +VehicleCommandSrv::~VehicleCommandSrv() +{ + +}; + +bool VehicleCommandSrv::process_request(ucdrBuffer *ub, const int64_t time_offset_us) +{ + vehicle_command_s data; + + if (ucdr_deserialize_vehicle_command(*ub, data, time_offset_us)) { + vehicle_command_pub_.publish(data); + is_reply_pending_ = true; + last_command_sent_ = data.command; + last_command_sent_timestamp_ = hrt_absolute_time(); + } + + return 0; +} + +bool VehicleCommandSrv::process_reply() +{ + vehicle_command_ack_s cmd_ack; + + if (is_reply_pending_ && vehicle_command_ack_sub_.update(&cmd_ack)) { + if (cmd_ack.command == last_command_sent_ && cmd_ack.timestamp > last_command_sent_timestamp_) { + last_command_sent_ = 0; + is_reply_pending_ = false; + + ucdrBuffer reply_ub; + const uint32_t topic_size = ucdr_topic_size_vehicle_command_ack(); + uint8_t reply_buffer[topic_size] = { + 0 + + }; + const int64_t time_offset_us = session_->time_offset / 1000; // ns -> us + ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer)); + ucdr_serialize_vehicle_command_ack(&cmd_ack, reply_ub, time_offset_us); + + uxr_buffer_reply(session_, reliable_out_stream_id_, replier_id_, &sample_id_, reply_buffer, sizeof(reply_buffer)); + } + } + + return 0; +} diff --git a/src/modules/uxrce_dds_client/vehicle_command_srv.h b/src/modules/uxrce_dds_client/vehicle_command_srv.h new file mode 100644 index 0000000000..4888b4f53e --- /dev/null +++ b/src/modules/uxrce_dds_client/vehicle_command_srv.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include +#include + +#include "srv_base.h" + +/** + * @see SrvBase + * @class VehicleCommandSrv The VehicleCommandSrv class implement the VehicleCommand service server. + */ +class VehicleCommandSrv : public SrvBase +{ +private: + uORB::Publication vehicle_command_pub_{ORB_ID(vehicle_command)}; + uORB::Subscription vehicle_command_ack_sub_{ORB_ID(vehicle_command_ack)}; + uint32_t last_command_sent_{0}; + hrt_abstime last_command_sent_timestamp_{0}; +public: + /** + * @brief Constructor. + * @see SrvBase. + * @param client_namespace namespace for the client service. + * @param index index used to create the replier id. + * @return Returns false iff successful, otherwise false. + */ + VehicleCommandSrv(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId input_stream_id, + uxrObjectId participant_id, const char *client_namespace, const uint8_t index); + + ~VehicleCommandSrv(); + + /** @see SrvBase */ + bool process_request(ucdrBuffer *ub, const int64_t time_offset_us); + + /** @see SrvBase */ + bool process_reply(); +};