From e773b927056cd53f35a13a7113891ace203a5e9c Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Fri, 3 May 2024 12:52:44 -0400 Subject: [PATCH] Fix service definition parsing on ROS rolling (#293) ### Changelog Fix service definition parsing on ROS rolling ### Description Prior to this change, we used the auto-generated `_Request.msg` and `_Response.msg` message definitions for parsing a service's request and response definition. However, these message definitions have been removed in https://github.com/ros2/rosidl/pull/753 causing foxglove bridge to raise warnings that the service definition could not be found. This PR changes the service definition parsing to lookup the service definition using the `.srv` file and splitting the content using the `===` seperator to obtain the request and response definition. --- CMakeLists.txt | 4 + .../foxglove_bridge/ros2_foxglove_bridge.hpp | 2 +- .../include/foxglove_bridge/utils.hpp | 26 +++ .../src/message_definition_cache.cpp | 169 +++++++++++------- .../src/ros2_foxglove_bridge.cpp | 6 +- ros2_foxglove_bridge/tests/smoke_test.cpp | 19 +- ros2_foxglove_bridge/tests/utils_test.cpp | 79 ++++++++ 7 files changed, 238 insertions(+), 67 deletions(-) create mode 100644 ros2_foxglove_bridge/tests/utils_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9603d36..a520562 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -243,6 +243,10 @@ elseif(ROS_BUILD_TYPE STREQUAL "ament_cmake") ament_target_dependencies(smoke_test rclcpp rclcpp_components std_msgs std_srvs) target_link_libraries(smoke_test foxglove_bridge_base) enable_strict_compiler_warnings(smoke_test) + + ament_add_gtest(utils_test ros2_foxglove_bridge/tests/utils_test.cpp) + target_link_libraries(utils_test foxglove_bridge_component) + enable_strict_compiler_warnings(utils_test) endif() endif() diff --git a/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp b/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp index 413e104..4a09ab6 100644 --- a/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp +++ b/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp @@ -109,7 +109,7 @@ class FoxgloveBridge : public rclcpp::Node { void logHandler(LogLevel level, char const* msg); void rosMessageHandler(const foxglove::ChannelId& channelId, ConnectionHandle clientHandle, - std::shared_ptr msg); + std::shared_ptr msg); void serviceRequest(const foxglove::ServiceRequest& request, ConnectionHandle clientHandle); diff --git a/ros2_foxglove_bridge/include/foxglove_bridge/utils.hpp b/ros2_foxglove_bridge/include/foxglove_bridge/utils.hpp index c5606be..86458a4 100644 --- a/ros2_foxglove_bridge/include/foxglove_bridge/utils.hpp +++ b/ros2_foxglove_bridge/include/foxglove_bridge/utils.hpp @@ -17,4 +17,30 @@ inline std::pair getNodeAndNodeNamespace(const std::st return std::make_pair(fqnNodeName.substr(0, found), fqnNodeName.substr(found + 1)); } +inline std::string trimString(std::string& str) { + constexpr char whitespaces[] = "\t\n\r "; + str.erase(0, str.find_first_not_of(whitespaces)); // trim left + str.erase(str.find_last_not_of(whitespaces) + 1); // trim right + return str; +} + +inline std::vector splitMessageDefinitions(std::istream& stream) { + std::vector definitions; + + std::string line = ""; + std::string definition = ""; + + while (std::getline(stream, line)) { + if (line == "---") { + definitions.push_back(trimString(definition)); + definition = ""; + } else { + definition += line + "\n"; + } + } + + definitions.push_back(trimString(definition)); + return definitions; +} + } // namespace foxglove_bridge diff --git a/ros2_foxglove_bridge/src/message_definition_cache.cpp b/ros2_foxglove_bridge/src/message_definition_cache.cpp index 7a17020..dae0195 100644 --- a/ros2_foxglove_bridge/src/message_definition_cache.cpp +++ b/ros2_foxglove_bridge/src/message_definition_cache.cpp @@ -14,6 +14,8 @@ #include #include +#include "foxglove_bridge/utils.hpp" + namespace foxglove { // Match datatype names (foo_msgs/Bar or foo_msgs/msg/Bar or foo_msgs/srv/Bar) @@ -118,21 +120,6 @@ static std::vector split_string(const std::string& str, return strings; } -/// @brief Split an action definition into individual goal, result and feedback definitions. -/// @param action_definition The full action definition as read from a .action file -/// @return A tuple holding goal, result and feedback definitions -static std::tuple split_action_msg_definition( - const std::string& action_definition) { - constexpr char SEP[] = "\n---\n"; - - const auto definitions = split_string(action_definition, SEP); - if (definitions.size() != 3) { - throw std::invalid_argument("Invalid action definition:\n" + action_definition); - } - - return {definitions[0], definitions[1], definitions[2]}; -} - inline bool ends_with(const std::string& str, const std::string& suffix) { return str.size() >= suffix.size() && 0 == str.compare(str.size() - suffix.size(), suffix.size(), suffix); @@ -156,6 +143,21 @@ std::string remove_action_subtype(const std::string action_type) { return action_type; } +std::string remove_service_subtype(const std::string service_type) { + const std::vector service_subtype_suffixes = { + SERVICE_REQUEST_MESSAGE_SUFFIX, + SERVICE_RESPONSE_MESSAGE_SUFFIX, + }; + + for (const auto& suffix : service_subtype_suffixes) { + if (ends_with(service_type, suffix)) { + return service_type.substr(0, service_type.length() - suffix.length()); + } + } + + return service_type; +} + MessageSpec::MessageSpec(MessageDefinitionFormat format, std::string text, const std::string& package_context) : dependencies(parse_dependencies(format, text, package_context)) @@ -168,6 +170,12 @@ const MessageSpec& MessageDefinitionCache::load_message_spec( it != msg_specs_by_definition_identifier_.end()) { return it->second; } + + if (definition_identifier.format == MessageDefinitionFormat::IDL) { + RCUTILS_LOG_ERROR_NAMED("foxglove_bridge", "IDL definitions are currently not supported"); + throw DefinitionNotFoundError(definition_identifier.package_resource_name); + } + std::smatch match; if (!std::regex_match(definition_identifier.package_resource_name, match, PACKAGE_TYPENAME_REGEX)) { @@ -177,14 +185,19 @@ const MessageSpec& MessageDefinitionCache::load_message_spec( const std::string package = match[1].str(); const std::string subfolder = match[2].str(); const std::string type_name = match[3].str(); - const bool is_action_type = subfolder == "action"; - // The action type name includes the subtype which we have to remove to get the action name. - // Type name: Fibonacci_FeedbackMessage -> Action name: Fibonacci - const std::string action_name = is_action_type ? remove_action_subtype(type_name) : ""; - const std::string filename = is_action_type - ? action_name + ".action" - : type_name + extension_for_format(definition_identifier.format); + std::string filename = ""; + if (subfolder == "action") { + // The action type name includes the subtype which we have to remove to get the action name. + // Type name: Fibonacci_FeedbackMessage -> Action name: Fibonacci + filename = remove_action_subtype(type_name) + ".action"; + } else if (subfolder == "srv") { + // The service type name includes the subtype which we have to remove to get the service name. + // Type name: SetBool_Request -> Service name: SetBool + filename = remove_service_subtype(type_name) + ".srv"; + } else { + filename = type_name + extension_for_format(definition_identifier.format); + } // Get the package share directory, or throw a PackageNotFoundError const std::string share_dir = ament_index_cpp::get_package_share_directory(package); @@ -211,59 +224,93 @@ const MessageSpec& MessageDefinitionCache::load_message_spec( if (!file.good()) { throw DefinitionNotFoundError(definition_identifier.package_resource_name); } - const std::string contents{std::istreambuf_iterator(file), {}}; - - if (is_action_type) { - if (definition_identifier.format == MessageDefinitionFormat::MSG) { - const auto [goalDef, resultDef, feedbackDef] = split_action_msg_definition(contents); - - // Define type definitions for each action subtype. - // These type definitions may include additional fields such as the goal_id. - // See also https://design.ros2.org/articles/actions.html - const std::map action_type_definitions = { - {ACTION_FEEDBACK_MESSAGE_SUFFIX, "unique_identifier_msgs/UUID goal_id\n" + feedbackDef}, - {std::string(ACTION_RESULT_SERVICE_SUFFIX) + SERVICE_REQUEST_MESSAGE_SUFFIX, - "unique_identifier_msgs/UUID goal_id\n"}, - {std::string(ACTION_RESULT_SERVICE_SUFFIX) + SERVICE_RESPONSE_MESSAGE_SUFFIX, - "int8 status\n" + resultDef}, - {std::string(ACTION_GOAL_SERVICE_SUFFIX) + SERVICE_REQUEST_MESSAGE_SUFFIX, - "unique_identifier_msgs/UUID goal_id\n" + goalDef}, - {std::string(ACTION_GOAL_SERVICE_SUFFIX) + SERVICE_RESPONSE_MESSAGE_SUFFIX, - "bool accepted\nbuiltin_interfaces/msg/Time stamp"}}; - - // Create a MessageSpec instance for every action subtype and add it to the cache. - for (const auto& [action_suffix, definition] : action_type_definitions) { - DefinitionIdentifier definition_id; - definition_id.format = definition_identifier.format; - definition_id.package_resource_name = package + "/action/" + action_name + action_suffix; - msg_specs_by_definition_identifier_.emplace( - definition_id, MessageSpec(definition_id.format, definition, package)); - } - // Find the the subtype that was originally requested and return it. - const auto it = msg_specs_by_definition_identifier_.find(definition_identifier); - if (it == msg_specs_by_definition_identifier_.end()) { - throw DefinitionNotFoundError(definition_identifier.package_resource_name); - } - return it->second; - } else { - RCUTILS_LOG_ERROR_NAMED("foxglove_bridge", - "Action IDL definitions are currently not supported"); + if (subfolder == "action") { + const auto split_definitions = foxglove_bridge::splitMessageDefinitions(file); + if (split_definitions.size() != 3) { + throw std::invalid_argument("Invalid action definition in " + filename + + ": Expected 3 definitions, got " + + std::to_string(split_definitions.size())); + } + + const auto& goalDef = split_definitions[0]; + const auto& resultDef = split_definitions[1]; + const auto& feedbackDef = split_definitions[2]; + + // Define type definitions for each action subtype. + // These type definitions may include additional fields such as the goal_id. + // See also https://design.ros2.org/articles/actions.html + const std::map action_type_definitions = { + {ACTION_FEEDBACK_MESSAGE_SUFFIX, "unique_identifier_msgs/UUID goal_id\n" + feedbackDef}, + {std::string(ACTION_RESULT_SERVICE_SUFFIX) + SERVICE_REQUEST_MESSAGE_SUFFIX, + "unique_identifier_msgs/UUID goal_id\n"}, + {std::string(ACTION_RESULT_SERVICE_SUFFIX) + SERVICE_RESPONSE_MESSAGE_SUFFIX, + "int8 status\n" + resultDef}, + {std::string(ACTION_GOAL_SERVICE_SUFFIX) + SERVICE_REQUEST_MESSAGE_SUFFIX, + "unique_identifier_msgs/UUID goal_id\n" + goalDef}, + {std::string(ACTION_GOAL_SERVICE_SUFFIX) + SERVICE_RESPONSE_MESSAGE_SUFFIX, + "bool accepted\nbuiltin_interfaces/msg/Time stamp"}}; + + // Create a MessageSpec instance for every action subtype and add it to the cache. + const std::string action_name = remove_action_subtype(type_name); + for (const auto& [action_suffix, definition] : action_type_definitions) { + DefinitionIdentifier definition_id; + definition_id.format = definition_identifier.format; + definition_id.package_resource_name = package + "/action/" + action_name + action_suffix; + msg_specs_by_definition_identifier_.emplace( + definition_id, MessageSpec(definition_id.format, definition, package)); + } + + // Find the the subtype that was originally requested and return it. + const auto it = msg_specs_by_definition_identifier_.find(definition_identifier); + if (it == msg_specs_by_definition_identifier_.end()) { + throw DefinitionNotFoundError(definition_identifier.package_resource_name); + } + return it->second; + } else if (subfolder == "srv") { + const auto split_definitions = foxglove_bridge::splitMessageDefinitions(file); + if (split_definitions.size() != 2) { + throw std::invalid_argument("Invalid service definition in " + filename + + ": Expected 2 definitions, got " + + std::to_string(split_definitions.size())); + } + + const auto& requestDef = split_definitions[0]; + const auto& responseDef = split_definitions[1]; + const std::map service_type_definitions = { + {SERVICE_REQUEST_MESSAGE_SUFFIX, requestDef}, {SERVICE_RESPONSE_MESSAGE_SUFFIX, responseDef}}; + + // Create a MessageSpec instance for both the request and response subtypes and add it to the + // cache. + const std::string service_name = remove_service_subtype(type_name); + for (const auto& [subType, definition] : service_type_definitions) { + DefinitionIdentifier definition_id; + definition_id.format = definition_identifier.format; + definition_id.package_resource_name = package + "/srv/" + service_name + subType; + msg_specs_by_definition_identifier_.emplace( + definition_id, MessageSpec(definition_id.format, definition, package)); + } + + // Find the the subtype that was originally requested and return it. + const auto it = msg_specs_by_definition_identifier_.find(definition_identifier); + if (it == msg_specs_by_definition_identifier_.end()) { throw DefinitionNotFoundError(definition_identifier.package_resource_name); } + return it->second; } else { // Normal message type. const MessageSpec& spec = msg_specs_by_definition_identifier_ .emplace(definition_identifier, - MessageSpec(definition_identifier.format, std::move(contents), package)) + MessageSpec(definition_identifier.format, + std::string{std::istreambuf_iterator(file), {}}, package)) .first->second; // "References and pointers to data stored in the container are only invalidated by erasing that // element, even when the corresponding iterator is invalidated." return spec; } -} +} // namespace foxglove std::pair MessageDefinitionCache::get_full_text( const std::string& root_package_resource_name) { diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp index 57042b4..19b2c7d 100644 --- a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -126,7 +126,7 @@ FoxgloveBridge::FoxgloveBridge(const rclcpp::NodeOptions& options) if (_useSimTime) { _clockSubscription = this->create_subscription( "/clock", rclcpp::QoS{rclcpp::KeepLast(1)}.best_effort(), - [&](std::shared_ptr msg) { + [&](std::shared_ptr msg) { const auto timestamp = rclcpp::Time{msg->clock}.nanoseconds(); assert(timestamp >= 0 && "Timestamp is negative"); _server->broadcastTime(static_cast(timestamp)); @@ -534,7 +534,7 @@ void FoxgloveBridge::subscribe(foxglove::ChannelId channelId, ConnectionHandle c try { auto subscriber = this->create_generic_subscription( topic, datatype, qos, - [this, channelId, clientHandle](std::shared_ptr msg) { + [this, channelId, clientHandle](std::shared_ptr msg) { this->rosMessageHandler(channelId, clientHandle, msg); }, subscriptionOptions); @@ -770,7 +770,7 @@ void FoxgloveBridge::logHandler(LogLevel level, char const* msg) { void FoxgloveBridge::rosMessageHandler(const foxglove::ChannelId& channelId, ConnectionHandle clientHandle, - std::shared_ptr msg) { + std::shared_ptr msg) { // NOTE: Do not call any RCLCPP_* logging functions from this function. Otherwise, subscribing // to `/rosout` will cause a feedback loop const auto timestamp = this->now().nanoseconds(); diff --git a/ros2_foxglove_bridge/tests/smoke_test.cpp b/ros2_foxglove_bridge/tests/smoke_test.cpp index f3bc256..be5522b 100644 --- a/ros2_foxglove_bridge/tests/smoke_test.cpp +++ b/ros2_foxglove_bridge/tests/smoke_test.cpp @@ -263,7 +263,7 @@ TEST(SmokeTest, testPublishing) { auto msgFuture = msgPromise.get_future(); auto node = rclcpp::Node::make_shared("tester"); auto sub = node->create_subscription( - advertisement.topic, 10, [&msgPromise](const std_msgs::msg::String::SharedPtr msg) { + advertisement.topic, 10, [&msgPromise](std::shared_ptr msg) { msgPromise.set_value(msg->data); }); rclcpp::executors::SingleThreadedExecutor executor; @@ -300,7 +300,7 @@ TEST_F(ExistingPublisherTest, testPublishingWithExistingPublisher) { auto msgFuture = msgPromise.get_future(); auto node = rclcpp::Node::make_shared("tester"); auto sub = node->create_subscription( - advertisement.topic, 10, [&msgPromise](const std_msgs::msg::String::SharedPtr msg) { + advertisement.topic, 10, [&msgPromise](std::shared_ptr msg) { msgPromise.set_value(msg->data); }); rclcpp::executors::SingleThreadedExecutor executor; @@ -537,6 +537,21 @@ TEST_F(ParameterTest, testGetParametersParallel) { } } +TEST_F(ServiceTest, testAdvertiseService) { + auto client = std::make_shared>(); + auto serviceFuture = foxglove::waitForService(client, SERVICE_NAME); + ASSERT_EQ(std::future_status::ready, client->connect(URI).wait_for(ONE_SECOND)); + ASSERT_EQ(std::future_status::ready, serviceFuture.wait_for(DEFAULT_TIMEOUT)); + const foxglove::Service service = serviceFuture.get(); + + EXPECT_EQ(service.name, SERVICE_NAME); + EXPECT_EQ(service.type, "std_srvs/srv/SetBool"); + EXPECT_EQ(service.requestSchema, "bool data # e.g. for hardware enabling / disabling"); + EXPECT_EQ(service.responseSchema, + "bool success # indicate successful run of triggered service\nstring message # " + "informational, e.g. for error messages"); +} + TEST_F(ServiceTest, testCallServiceParallel) { // Connect a few clients (in parallel) and make sure that they can all call the service auto clients = { diff --git a/ros2_foxglove_bridge/tests/utils_test.cpp b/ros2_foxglove_bridge/tests/utils_test.cpp new file mode 100644 index 0000000..a7c6992 --- /dev/null +++ b/ros2_foxglove_bridge/tests/utils_test.cpp @@ -0,0 +1,79 @@ +#include + +#include + +TEST(SplitDefinitionsTest, EmptyMessageDefinition) { + const std::string messageDef = ""; + std::istringstream stream(messageDef); + const auto definitions = foxglove_bridge::splitMessageDefinitions(stream); + ASSERT_EQ(definitions.size(), 1u); + EXPECT_EQ(definitions[0], ""); +} + +TEST(SplitDefinitionsTest, ServiceDefinition) { + const std::string messageDef = + "bool data\n" + "---\n" + "bool success "; + std::istringstream stream(messageDef); + const auto definitions = foxglove_bridge::splitMessageDefinitions(stream); + ASSERT_EQ(definitions.size(), 2u); + EXPECT_EQ(definitions[0], "bool data"); + EXPECT_EQ(definitions[1], "bool success"); +} + +TEST(SplitDefinitionsTest, ServiceDefinitionEmptyRequest) { + const std::string messageDef = + "---\n" + "bool success "; + std::istringstream stream(messageDef); + const auto definitions = foxglove_bridge::splitMessageDefinitions(stream); + ASSERT_EQ(definitions.size(), 2u); + EXPECT_EQ(definitions[0], ""); + EXPECT_EQ(definitions[1], "bool success"); +} + +TEST(SplitDefinitionsTest, ServiceDefinitionEmptyResponse) { + const std::string messageDef = + "bool data\n" + "---"; + std::istringstream stream(messageDef); + const auto definitions = foxglove_bridge::splitMessageDefinitions(stream); + ASSERT_EQ(definitions.size(), 2u); + EXPECT_EQ(definitions[0], "bool data"); + EXPECT_EQ(definitions[1], ""); +} + +TEST(SplitDefinitionsTest, ActionDefinition) { + const std::string messageDef = + "bool data\n" + "---\n" + "bool success\n" + "---\n" + "bool feedback"; + std::istringstream stream(messageDef); + const auto definitions = foxglove_bridge::splitMessageDefinitions(stream); + ASSERT_EQ(definitions.size(), 3u); + EXPECT_EQ(definitions[0], "bool data"); + EXPECT_EQ(definitions[1], "bool success"); + EXPECT_EQ(definitions[2], "bool feedback"); +} + +TEST(SplitDefinitionsTest, ActionDefinitionNoGoal) { + const std::string messageDef = + "bool data\n" + "---\n" + "---\n" + "bool feedback"; + std::istringstream stream(messageDef); + const auto definitions = foxglove_bridge::splitMessageDefinitions(stream); + ASSERT_EQ(definitions.size(), 3u); + EXPECT_EQ(definitions[0], "bool data"); + EXPECT_EQ(definitions[1], ""); + EXPECT_EQ(definitions[2], "bool feedback"); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}