add services

px4_msgs_old/rename_msg_type.py.in

@@ -19,6 +19,10 @@ def replace_namespace_and_type(content: str):
     content = content.replace('"px4_msgs_old::msg"', '"px4_msgs::msg"')
     # Replace versioned type with non-versioned one
     content = re.sub(r'("[a-zA-Z0-9]+)V[0-9]+"', '\\1"', content)
+    # Services
+    content = content.replace('"px4_msgs_old::srv"', '"px4_msgs::srv"')
+    content = re.sub(r'("[a-zA-Z0-9]+)V[0-9]+_Request"', '\\1_Request"', content)
+    content = re.sub(r'("[a-zA-Z0-9]+)V[0-9]+_Response"', '\\1_Response"', content)
     return content
 
 with open(json_file, 'r') as f:

translation_node/CMakeLists.txt

@@ -11,9 +11,17 @@ find_package(rclcpp REQUIRED)
 find_package(px4_msgs REQUIRED)
 find_package(px4_msgs_old REQUIRED)
 
+if(DEFINED ENV{ROS_DISTRO})
+  set(ROS_DISTRO $ENV{ROS_DISTRO})
+else()
+  set(ROS_DISTRO "rolling")
+endif()
+
+
 add_library(${PROJECT_NAME}_lib
         src/monitor.cpp
         src/pub_sub_graph.cpp
+        src/service_graph.cpp
         src/translations.cpp
 )
 ament_target_dependencies(${PROJECT_NAME}_lib rclcpp px4_msgs px4_msgs_old)
@@ -26,6 +34,11 @@ install(TARGETS
         ${PROJECT_NAME}
         DESTINATION lib/${PROJECT_NAME})
 
+if(${ROS_DISTRO} STREQUAL "humble")
+  message(WARNING "Disabling services for ROS humble (API is not supported)")
+  target_compile_definitions(${PROJECT_NAME}_lib PRIVATE DISABLE_SERVICES)
+endif()
+
 if(BUILD_TESTING)
   find_package(std_msgs REQUIRED)
   find_package(ament_lint_auto REQUIRED)

translation_node/src/main.cpp

@@ -9,7 +9,9 @@
 #include "vehicle_attitude_v2.h"
 #include "vehicle_attitude_v3.h"
 #include "vehicle_local_global_position_v2.h"
+#include "vehicle_command_service_v1.h"
 #include "pub_sub_graph.h"
+#include "service_graph.h"
 #include "monitor.h"
 
 using namespace std::chrono_literals;
@@ -19,12 +21,14 @@ class RosTranslationNode : public rclcpp::Node
 public:
 	RosTranslationNode() : Node("translation_node")
 	{
-		_pub_sub_graph = std::make_unique<PubSubGraph>(*this, RegisteredTranslations::instance().translations());
-		_monitor = std::make_unique<Monitor>(*this, *_pub_sub_graph);
+		_pub_sub_graph = std::make_unique<PubSubGraph>(*this, RegisteredTranslations::instance().topicTranslations());
+		_service_graph = std::make_unique<ServiceGraph>(*this, RegisteredTranslations::instance().serviceTranslations());
+		_monitor = std::make_unique<Monitor>(*this, *_pub_sub_graph, *_service_graph);
 	}
 
 private:
 	std::unique_ptr<PubSubGraph> _pub_sub_graph;
+	std::unique_ptr<ServiceGraph> _service_graph;
 	rclcpp::TimerBase::SharedPtr _node_update_timer;
 	std::unique_ptr<Monitor> _monitor;
 };

translation_node/src/monitor.cpp

@@ -5,8 +5,8 @@
 #include "monitor.h"
 using namespace std::chrono_literals;
 
-Monitor::Monitor(rclcpp::Node &node, PubSubGraph& pub_sub_graph)
-	: _node(node), _pub_sub_graph(pub_sub_graph) {
+Monitor::Monitor(rclcpp::Node &node, PubSubGraph& pub_sub_graph, ServiceGraph& service_graph)
+	: _node(node), _pub_sub_graph(pub_sub_graph), _service_graph(service_graph) {
 
 	// Monitor subscriptions & publishers
 	// TODO: event-based
@@ -16,6 +16,8 @@ Monitor::Monitor(rclcpp::Node &node, PubSubGraph& pub_sub_graph)
 }
 
 void Monitor::updateNow() {
+
+	// Topics
 	std::vector<PubSubGraph::TopicInfo> topic_info;
 	const auto topics = _node.get_topic_names_and_types();
 	for (const auto& [topic_name, topic_types] : topics) {
@@ -36,4 +38,19 @@ void Monitor::updateNow() {
 		}
 	}
 	_pub_sub_graph.updateCurrentTopics(topic_info);
+
+#ifndef DISABLE_SERVICES // ROS Humble does not support the count_services() call
+	// Services
+	std::vector<ServiceGraph::ServiceInfo> service_info;
+	const auto services = _node.get_service_names_and_types();
+	for (const auto& [service_name, service_types] : services) {
+		const int num_services = _node.get_node_graph_interface()->count_services(service_name);
+		const int num_clients = _node.get_node_graph_interface()->count_clients(service_name);
+		// We cannot filter out our own node, as we don't have that info.
+		// We could use `get_service_names_and_types_by_node`, but then we would not get
+		// services by non-ros nodes (e.g. microxrce dds bridge)
+		service_info.emplace_back(ServiceGraph::ServiceInfo{service_name, num_services, num_clients});
+	}
+	_service_graph.updateCurrentServices(service_info);
+#endif
 }

translation_node/src/monitor.h

@@ -6,16 +6,18 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include "pub_sub_graph.h"
+#include "service_graph.h"
 #include <functional>
 
 class Monitor {
 public:
-	explicit Monitor(rclcpp::Node &node, PubSubGraph& pub_sub_graph);
+	explicit Monitor(rclcpp::Node &node, PubSubGraph& pub_sub_graph, ServiceGraph& service_graph);
 
 	void updateNow();
 
 private:
 	rclcpp::Node &_node;
 	PubSubGraph& _pub_sub_graph;
+	ServiceGraph& _service_graph;
 	rclcpp::TimerBase::SharedPtr _node_update_timer;
 };

translation_node/src/pub_sub_graph.cpp

@@ -5,7 +5,7 @@
 #include "pub_sub_graph.h"
 #include "util.h"
 
-PubSubGraph::PubSubGraph(rclcpp::Node &node, const Translations &translations) : _node(node) {
+PubSubGraph::PubSubGraph(rclcpp::Node &node, const TopicTranslations &translations) : _node(node) {
 
 	std::unordered_map<std::string, std::set<MessageVersionType>> known_versions;
 
@@ -19,15 +19,16 @@ PubSubGraph::PubSubGraph(rclcpp::Node &node, const Translations &translations) :
 		known_versions[full_topic_name].insert(id.version);
 	}
 
-	for (const auto& translation : translations.translations()) {
-		std::vector<MessageIdentifier> inputs = translation.inputs;
-		for (auto& input : inputs) {
-			input.topic_name = getFullTopicName(_node.get_effective_namespace(), input.topic_name);
-		}
-		std::vector<MessageIdentifier> outputs = translation.outputs;
-		for (auto& output : outputs) {
-			output.topic_name = getFullTopicName(_node.get_effective_namespace(), output.topic_name);
+	auto get_full_topic_names = [this](std::vector<MessageIdentifier> ids) {
+		for (auto& id : ids) {
+			id.topic_name = getFullTopicName(_node.get_effective_namespace(), id.topic_name);
 		}
+		return ids;
+	};
+
+	for (const auto& translation : translations.translations()) {
+		const std::vector<MessageIdentifier> inputs = get_full_topic_names(translation.inputs);
+		const std::vector<MessageIdentifier> outputs = get_full_topic_names(translation.outputs);
 		_pub_sub_graph.addTranslation(translation.cb, inputs, outputs);
 	}
 

translation_node/src/pub_sub_graph.h

@@ -18,7 +18,7 @@ class PubSubGraph {
 		int num_publishers; ///< does not include this node's publishers
 	};
 
-	PubSubGraph(rclcpp::Node& node, const Translations& translations);
+	PubSubGraph(rclcpp::Node& node, const TopicTranslations& translations);
 
 	void updateCurrentTopics(const std::vector<TopicInfo>& topics);