From 67678ff7ec56b5f2984467932292b6c0093eaf6c Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 12 Sep 2023 10:28:31 +0800 Subject: [PATCH] Address reivew comments Signed-off-by: Barry Xu --- docs/message_definition_encoding.md | 2 +- ros2bag/ros2bag/api/__init__.py | 23 +++++++++++++- ros2bag/ros2bag/verb/burst.py | 8 ++--- ros2bag/ros2bag/verb/play.py | 31 +++++++++++++------ ros2bag/ros2bag/verb/record.py | 16 +++++----- rosbag2_cpp/include/rosbag2_cpp/info.hpp | 6 ++-- .../include/rosbag2_cpp/service_utils.hpp | 20 +++++++++--- rosbag2_cpp/src/rosbag2_cpp/info.cpp | 12 +++---- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 12 ++++--- .../rosbag2_cpp/writers/sequential_writer.cpp | 4 +-- .../test/rosbag2_cpp/test_service_utils.cpp | 7 +++-- rosbag2_py/src/rosbag2_py/_info.cpp | 2 +- .../src/rosbag2_py/format_service_info.cpp | 5 +-- .../src/rosbag2_py/format_service_info.hpp | 2 +- .../src/rosbag2_transport/player.cpp | 12 +++++-- 15 files changed, 107 insertions(+), 55 deletions(-) diff --git a/docs/message_definition_encoding.md b/docs/message_definition_encoding.md index 2c9d5db6ff..7d6f5a7554 100644 --- a/docs/message_definition_encoding.md +++ b/docs/message_definition_encoding.md @@ -16,7 +16,7 @@ This set of definitions with all field types recursively included can be called ## `ros2msg` encoding -This encoding consists of definitions in [.msg](https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html#message-description-specification) and [.srv](https://docs.ros.org/en/rolling/Concepts/Basic/About-Interfaces.html#services) format, concatenated together in human-readable form with +This encoding consists of definitions in [.msg](https://docs.ros.org/en/rolling/Concepts/Basic/About-Interfaces.html#messages) and [.srv](https://docs.ros.org/en/rolling/Concepts/Basic/About-Interfaces.html#services) format, concatenated together in human-readable form with a delimiter. The top-level message definition is present first, with no delimiter. All dependent .msg definitions are preceded by a two-line delimiter: diff --git a/ros2bag/ros2bag/api/__init__.py b/ros2bag/ros2bag/api/__init__.py index 0c9a192ed2..5fdcb55a8e 100644 --- a/ros2bag/ros2bag/api/__init__.py +++ b/ros2bag/ros2bag/api/__init__.py @@ -55,7 +55,15 @@ def _split_lines(self, text, width): def print_error(string: str) -> str: - return '[ERROR] [ros2bag]: {}'.format(string) + return _print_base('ERROR', string) + + +def print_warn(string: str) -> str: + return _print_base('WARN', string) + + +def _print_base(print_type: str, string: str) -> str: + return '[{}] [ros2bag]: {}'.format(print_type, string) def dict_to_duration(time_dict: Optional[Dict[str, int]]) -> Duration: @@ -200,3 +208,16 @@ def add_writer_storage_plugin_extensions(parser: ArgumentParser) -> None: 'Settings in this profile can still be overridden by other explicit options ' 'and --storage-config-file. Profiles:\n' + '\n'.join([f'{preset[0]}: {preset[1]}' for preset in preset_profiles])) + + +def convert_service_to_service_event_topic(services): + services_event_topics = [] + + if not services: + return services_event_topics + + for service in services: + name = '/' + service if service[0] != '/' else service + services_event_topics.append(name + '/_service_event') + + return services_event_topics diff --git a/ros2bag/ros2bag/verb/burst.py b/ros2bag/ros2bag/verb/burst.py index bbd9f8589c..26547adb53 100644 --- a/ros2bag/ros2bag/verb/burst.py +++ b/ros2bag/ros2bag/verb/burst.py @@ -18,6 +18,7 @@ from ros2bag.api import add_standard_reader_args from ros2bag.api import check_not_negative_int from ros2bag.api import check_positive_float +from ros2bag.api import convert_service_to_service_event_topic from ros2bag.api import convert_yaml_to_qos_profile from ros2bag.api import print_error from ros2bag.verb import VerbExtension @@ -95,12 +96,7 @@ def main(self, *, args): # noqa: D102 play_options.rate = 1.0 play_options.topics_to_filter = args.topics # Convert service name to service event topic name - services = [] - if args.services and len(args.services) != 0: - for s in args.services: - name = '/' + s if s[0] != '/' else s - services.append(name + '/_service_event') - play_options.services_to_filter = services + play_options.services_to_filter = convert_service_to_service_event_topic(args.services) play_options.topic_qos_profile_overrides = qos_profile_overrides play_options.loop = False play_options.topic_remapping_options = topic_remapping diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index e25b720cb6..fe15a41c12 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -18,8 +18,10 @@ from ros2bag.api import add_standard_reader_args from ros2bag.api import check_not_negative_int from ros2bag.api import check_positive_float +from ros2bag.api import convert_service_to_service_event_topic from ros2bag.api import convert_yaml_to_qos_profile from ros2bag.api import print_error +from ros2bag.api import print_warn from ros2bag.verb import VerbExtension from ros2cli.node import NODE_NAME_PREFIX from rosbag2_py import Player @@ -54,12 +56,17 @@ def add_arguments(self, parser, cli_name): # noqa: D102 'replayed.') parser.add_argument( '--services', type=str, default=[], nargs='+', - help='services to replay, separated by space. At least one service needs to be ' - 'specified.') + help='services to replay, separated by space. if none specified, all services will ' + 'be played.') parser.add_argument( '-e', '--regex', default='', help='filter topics by regular expression to replay, separated by space. If none ' 'specified, all topics will be replayed.') + parser.add_argument( + '-x', '--exclude', default='', + help='regular expressions to exclude topics from replay, separated by space. If none ' + 'specified, all topics will be replayed. This argument is deprecated and please ' + 'use --exclude-topics.') parser.add_argument( '--exclude-topics', default='', help='regular expressions to exclude topics from replay, separated by space. If none ' @@ -172,6 +179,10 @@ def main(self, *, args): # noqa: D102 except (InvalidQoSProfileException, ValueError) as e: return print_error(str(e)) + if args.exclude and args.exclude_topics: + return print_error(str('-x/--exclude and --exclude_topics cannot be used at the ' + 'same time.')) + storage_config_file = '' if args.storage_config_file: storage_config_file = args.storage_config_file.name @@ -193,15 +204,17 @@ def main(self, *, args): # noqa: D102 play_options.topics_to_filter = args.topics # Convert service name to service event topic name - services = [] - if args.services and len(args.services) != 0: - for s in args.services: - name = '/' + s if s[0] != '/' else s - services.append(name + '/_service_event') - play_options.services_to_filter = services + play_options.services_to_filter = convert_service_to_service_event_topic(args.services) play_options.regex_to_filter = args.regex - play_options.topics_regex_to_exclude = args.exclude_topics + + if args.exclude: + print(print_warn(str('-x/--exclude argument is deprecated. Please use ' + '--exclude-topics.'))) + play_options.topics_regex_to_exclude = args.exclude + else: + play_options.topics_regex_to_exclude = args.exclude_topics + if args.exclude_services: play_options.services_regex_to_exclude = args.exclude_services + '/_service_event' else: diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index dce1361dc7..3b3e0ed1da 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -18,6 +18,7 @@ from rclpy.qos import InvalidQoSProfileException from ros2bag.api import add_writer_storage_plugin_extensions +from ros2bag.api import convert_service_to_service_event_topic from ros2bag.api import convert_yaml_to_qos_profile from ros2bag.api import print_error from ros2bag.api import SplitLineFormatter @@ -183,11 +184,17 @@ def add_arguments(self, parser, cli_name): # noqa: D102 help='Choose the compression format/algorithm. ' 'Has no effect if no compression mode is chosen. Default: %(default)s.') - def main(self, *, args): # noqa: D102 + def _check_necessary_argument(self, args): # One options out of --all, --all-topics, --all-services, --services, topics or --regex # must be used if not (args.all or args.all_topics or args.all_services or args.services or (args.topics and len(args.topics) > 0) or args.regex): + return False + return True + + def main(self, *, args): # noqa: D102 + + if not self._check_necessary_argument(args): return print_error('Must specify only one option out of --all, --all-topics, ' '--all-services, --services, topics and --regex') @@ -286,12 +293,7 @@ def main(self, *, args): # noqa: D102 record_options.all_services = args.all_services or args.all # Convert service name to service event topic name - services = [] - if args.services and len(args.services) != 0: - for s in args.services: - name = '/' + s if s[0] != '/' else s - services.append(name + '/_service_event') - record_options.services = services + record_options.services = convert_service_to_service_event_topic(args.services) recorder = Recorder() diff --git a/rosbag2_cpp/include/rosbag2_cpp/info.hpp b/rosbag2_cpp/include/rosbag2_cpp/info.hpp index 3d109d4b38..a251598ea5 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/info.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/info.hpp @@ -26,14 +26,14 @@ namespace rosbag2_cpp { -typedef struct +typedef ROSBAG2_CPP_PUBLIC_TYPE struct rosbag2_service_info_t { std::string name; std::string type; std::string serialization_format; size_t request_count; size_t response_count; -} service_info; +} rosbag2_service_info_t; class ROSBAG2_CPP_PUBLIC Info { @@ -43,7 +43,7 @@ class ROSBAG2_CPP_PUBLIC Info virtual rosbag2_storage::BagMetadata read_metadata( const std::string & uri, const std::string & storage_id = ""); - virtual std::vector> read_service_info( + virtual std::vector> read_service_info( const std::string & uri, const std::string & storage_id = ""); }; diff --git a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp index f6ddce86a2..188d18396c 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp @@ -17,15 +17,25 @@ #include +#include "rosbag2_cpp/visibility_control.hpp" + namespace rosbag2_cpp { -bool is_service_event_topic(const std::string & topic, const std::string & topic_type); +ROSBAG2_CPP_PUBLIC +bool +is_service_event_topic(const std::string & topic, const std::string & topic_type); -std::string service_event_topic_name_to_service_name(const std::string & topic_name); +ROSBAG2_CPP_PUBLIC +std::string +service_event_topic_name_to_service_name(const std::string & topic_name); -std::string service_event_topic_type_to_service_type(const std::string & topic_type); +ROSBAG2_CPP_PUBLIC +std::string +service_event_topic_type_to_service_type(const std::string & topic_type); -size_t get_serialization_size_for_service_metadata_event(); -} +ROSBAG2_CPP_PUBLIC +size_t +get_serialization_size_for_service_metadata_event(); +} // namespace rosbag2_cpp #endif // ROSBAG2_CPP__SERVICE_UTILS_HPP_ diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index c91a8a4a99..cf4faade5c 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "rosbag2_cpp/info.hpp" + #include #include #include @@ -22,7 +23,6 @@ #include "rmw/rmw.h" #include "rcpputils/filesystem_helper.hpp" -#include "rosbag2_cpp/info.hpp" #include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/logging.hpp" #include "rosbag2_storage/metadata_io.hpp" @@ -84,7 +84,7 @@ struct service_req_resp_info }; } // namespace -std::vector> Info::read_service_info( +std::vector> Info::read_service_info( const std::string & uri, const std::string & storage_id) { rosbag2_storage::StorageFactory factory; @@ -96,13 +96,13 @@ std::vector> Info::read_service_info( using service_analysis = std::unordered_map>; - std::unordered_map> all_service_info; + std::unordered_map> all_service_info; service_analysis service_process_info; auto all_topics_types = storage->get_all_topics_and_types(); for (auto & t : all_topics_types) { if (is_service_event_topic(t.name, t.type)) { - auto service_info = std::make_shared(); + auto service_info = std::make_shared(); service_info->name = service_event_topic_name_to_service_name(t.name); service_info->type = service_event_topic_type_to_service_type(t.type); service_info->serialization_format = t.serialization_format; @@ -111,7 +111,7 @@ std::vector> Info::read_service_info( } } - std::vector> ret_service_info; + std::vector> ret_service_info; if (!all_service_info.empty()) { auto msg = service_msgs::msg::ServiceEventInfo(); diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index e658db7b97..19cecf3fd5 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -46,11 +46,15 @@ bool is_service_event_topic(const std::string & topic, const std::string & topic return false; } - std::string end_topic_type = topic_type.substr( - topic_type.length() - std::strlen(service_event_topic_type_postfix)); + if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { + return false; + } - return end_topic_name == RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX && - end_topic_type == service_event_topic_type_postfix; + return (topic_type.compare( + topic_type.length() - std::strlen(service_event_topic_type_postfix), + std::strlen(service_event_topic_type_postfix), + service_event_topic_type_postfix) == 0) && + (end_topic_name == RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); } std::string service_event_topic_name_to_service_name(const std::string & topic_name) diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 77d7b04618..acb36eb893 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -200,9 +200,7 @@ void SequentialWriter::create_topic(const rosbag2_storage::TopicMetadata & topic std::string topic_type; if (is_service_event_topic(topic_with_type.name, topic_with_type.type)) { // change service event type to service type for next step to get message definition - topic_type = topic_with_type.type.substr( - 0, - topic_with_type.type.length() - strlen("_Event")); + topic_type = service_event_topic_type_to_service_type(topic_with_type.type); } else { topic_type = topic_with_type.type; } diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp index b04c0e6afd..51420e20ac 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include "rosbag2_cpp/service_utils.hpp" @@ -26,7 +27,7 @@ class ServiceUtilsTest : public Test TEST_F(ServiceUtilsTest, check_is_service_event_topic) { - std::vector, bool>> all_test_data = + std::vector, bool>> all_test_data = { {{"/abc/_service_event", "package/srv/xyz_Event"}, true}, {{"/_service_event", "package/srv/xyz_Event"}, false}, @@ -37,8 +38,8 @@ TEST_F(ServiceUtilsTest, check_is_service_event_topic) for (const auto & test_data : all_test_data) { EXPECT_TRUE( - rosbag2_cpp::is_service_event_topic(test_data.first.first, test_data.first.second) == - test_data.second); + rosbag2_cpp::is_service_event_topic( + std::get<0>(test_data.first), std::get<1>(test_data.first)) == test_data.second); } } diff --git a/rosbag2_py/src/rosbag2_py/_info.cpp b/rosbag2_py/src/rosbag2_py/_info.cpp index 061e7317c3..f6e0f428a1 100644 --- a/rosbag2_py/src/rosbag2_py/_info.cpp +++ b/rosbag2_py/src/rosbag2_py/_info.cpp @@ -46,7 +46,7 @@ class Info { auto metadata_info = read_metadata(uri, storage_id); - std::vector> all_services_info; + std::vector> all_services_info; for (auto & file_info : metadata_info.files) { auto services_info = info_->read_service_info( uri + "/" + file_info.path, diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.cpp b/rosbag2_py/src/rosbag2_py/format_service_info.cpp index 07ee80edec..0c1366bee9 100644 --- a/rosbag2_py/src/rosbag2_py/format_service_info.cpp +++ b/rosbag2_py/src/rosbag2_py/format_service_info.cpp @@ -17,7 +17,8 @@ #include "format_service_info.hpp" std::string -format_service_info(std::vector> & service_info_list) +format_service_info( + std::vector> & service_info_list) { std::stringstream info_stream; int indentation_spaces = 21; @@ -29,7 +30,7 @@ format_service_info(std::vector> & se } auto print_service_info = - [&info_stream](const std::shared_ptr & si) -> void { + [&info_stream](const std::shared_ptr & si) -> void { info_stream << "Service: " << si->name << " | "; info_stream << "Type: " << si->type << " | "; info_stream << "Request Count: " << si->request_count << " | "; diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.hpp b/rosbag2_py/src/rosbag2_py/format_service_info.hpp index f1aac050a4..7337e5be4e 100644 --- a/rosbag2_py/src/rosbag2_py/format_service_info.hpp +++ b/rosbag2_py/src/rosbag2_py/format_service_info.hpp @@ -22,6 +22,6 @@ #include "rosbag2_cpp/info.hpp" std::string format_service_info( - std::vector> & service_info); + std::vector> & service_info); #endif // ROSBAG2_PY__FORMAT_SERVICE_INFO_HPP_ diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 343271943d..49999c86ad 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -1103,13 +1103,15 @@ void Player::PlayerClient::async_send_request(const rclcpp::SerializedMessage & "'%s' service isn't ready !", service_name_.c_str()); } - } else { - throw std::runtime_error( - "Failed to deserialize service event message for " + service_name_ + " !"); } message_members_->fini_function(ros_message); delete[] static_cast(ros_message); + + if (ret != RMW_RET_OK) { + throw std::runtime_error( + "Failed to deserialize service event message for " + service_name_ + " !"); + } } uint8_t Player::PlayerClient::get_msg_event_type(const rclcpp::SerializedMessage & message) @@ -1119,6 +1121,10 @@ uint8_t Player::PlayerClient::get_msg_event_type(const rclcpp::SerializedMessage const rosidl_message_type_support_t * type_support_info = rosidl_typesupport_cpp:: get_message_type_support_handle(); + if (type_support_info == nullptr) { + throw std::runtime_error( + "It failed to get message type support handle of service event info !"); + } auto ret = rmw_deserialize( &message.get_rcl_serialized_message(),