[uxrce_dds_client] add services (repliers) and basic VehicleCommand service

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>

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

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
 

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:

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<cstdio>
+//#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;
+}

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 <uxr/client/client.h>
+
+/** @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);
+};

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 <uxr/client/client.h>
 #include <uxr/client/util/ping.h>
 #include <ucdr/microcdr.h>
@@ -73,8 +76,6 @@ void on_time(uxrSession *session, int64_t current_time, int64_t received_timesta
 		Timesync *timesync = static_cast<Timesync *>(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");

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 <lib/timesync/Timesync.hpp>
 
+#include "srv_base.h"
+
+#define MAX_NUM_REPLIERS 5
+
 class UxrceddsClient : public ModuleBase<UxrceddsClient>, 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};

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 <uORB/ucdr/vehicle_command.h>
+#include <uORB/ucdr/vehicle_command_ack.h>
+
+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<vehicle_command_s>::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;
+}

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 <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+
+#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_s> 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();
+};