Skip to content

Commit

Permalink
Updated code on exclude parameters
Browse files Browse the repository at this point in the history
Refer to ros2#1483

Signed-off-by: Barry Xu <barry.xu@sony.com>
  • Loading branch information
Barry-Xu-2018 committed Nov 14, 2023
1 parent df79ef4 commit b6f732e
Show file tree
Hide file tree
Showing 16 changed files with 263 additions and 64 deletions.
21 changes: 16 additions & 5 deletions ros2bag/ros2bag/verb/record.py
Original file line number Diff line number Diff line change
Expand Up @@ -79,12 +79,16 @@ def add_arguments(self, parser, cli_name): # noqa: D102
'Overrides --all, --all-topics and --all-services, applies on top of '
'topics list and service list.')
parser.add_argument(
'--exclude-topics', default='',
help='Exclude topics containing provided regular expression. '
'--exclude-regex', default='',
help='Exclude topics and services containing provided regular expression. '
'Works on top of --all, --all-topics, or --regex.')
parser.add_argument(
'--exclude-services', default='',
help='Exclude services containing provided regular expression. '
'--exclude-topics', type=str, metavar='Topic', nargs='+',
help='List of topics not being recorded. '
'Works on top of --all, --all-topics, or --regex.')
parser.add_argument(
'--exclude-services', type=str, metavar='ServiceName', nargs='+',
help='List of services not being recorded. '
'Works on top of --all, --all-services, or --regex.')

# Enable to record service
Expand Down Expand Up @@ -212,6 +216,11 @@ def main(self, *, args): # noqa: D102
return print_error('Must specify only one option out of --all, --all-topics, '
'topics or --regex')

if (args.exclude_regex and \
not (args.regex or args.all or args.all_topics or args.all_services)):
return print_error('--exclude-regex argument requires either --all, --all, '
'--all-topics, --all-services or --regex')

if args.exclude_topics and not (args.regex or args.all or args.all_topics):
return print_error('--exclude-topics argument requires either --all, --all-topics '
'or --regex')
Expand Down Expand Up @@ -277,8 +286,10 @@ def main(self, *, args): # noqa: D102
record_options.topic_polling_interval = datetime.timedelta(
milliseconds=args.polling_interval)
record_options.regex = args.regex
record_options.exclude_regex = args.exclude_regex
record_options.exclude_topics = args.exclude_topics
record_options.exclude_services = args.exclude_services
record_options.exclude_services = \
convert_service_to_service_event_topic(args.exclude_services)
record_options.node_prefix = NODE_NAME_PREFIX
record_options.compression_mode = args.compression_mode
record_options.compression_format = args.compression_format
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,11 +35,12 @@ struct RecordOptions
bool is_discovery_disabled = false;
std::vector<std::string> topics;
std::vector<std::string> services; // service event topic
std::vector<std::string> exclude_topics;
std::vector<std::string> exclude_services; // service event topic
std::string rmw_serialization_format;
std::chrono::milliseconds topic_polling_interval{100};
std::string regex = "";
std::string exclude_topics = "";
std::string exclude_services = "";
std::string exclude_regex = "";
std::string node_prefix = "";
std::string compression_mode = "";
std::string compression_format = "";
Expand Down
7 changes: 5 additions & 2 deletions rosbag2_transport/src/rosbag2_transport/record_options.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ Node convert<rosbag2_transport::RecordOptions>::encode(
node["rmw_serialization_format"] = record_options.rmw_serialization_format;
node["topic_polling_interval"] = record_options.topic_polling_interval;
node["regex"] = record_options.regex;
node["exclude_regex"] = record_options.exclude_regex;
node["exclude_topics"] = record_options.exclude_topics;
node["exclude_services"] = record_options.exclude_services;
node["node_prefix"] = record_options.node_prefix;
Expand Down Expand Up @@ -80,8 +81,10 @@ bool convert<rosbag2_transport::RecordOptions>::decode(
optional_assign<std::chrono::milliseconds>(
node, "topic_polling_interval", record_options.topic_polling_interval);
optional_assign<std::string>(node, "regex", record_options.regex);
optional_assign<std::string>(node, "exclude_topics", record_options.exclude_topics);
optional_assign<std::string>(node, "exclude_services", record_options.exclude_services);
optional_assign<std::string>(node, "exclude_regex", record_options.exclude_regex);
optional_assign<std::vector<std::string>>(node, "exclude_topics", record_options.exclude_topics);
optional_assign<std::vector<std::string>>(
node, "exclude_services", record_options.exclude_services);
optional_assign<std::string>(node, "node_prefix", record_options.node_prefix);
optional_assign<std::string>(node, "compression_mode", record_options.compression_mode);
optional_assign<std::string>(node, "compression_format", record_options.compression_format);
Expand Down
26 changes: 19 additions & 7 deletions rosbag2_transport/src/rosbag2_transport/topic_filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -162,9 +162,8 @@ bool TopicFilter::take_topic(
return false;
}

std::regex exclude_topics_regex(record_options_.exclude_topics);
if (!record_options_.exclude_topics.empty() &&
std::regex_search(topic_name, exclude_topics_regex))
topic_in_list(topic_name, record_options_.exclude_topics))
{
return false;
}
Expand All @@ -177,6 +176,13 @@ bool TopicFilter::take_topic(
return false;
}

if (!record_options_.exclude_regex.empty()) {
std::regex exclude_regex(record_options_.exclude_regex);
if (std::regex_search(topic_name, exclude_regex)) {
return false;
}
}

if (!record_options_.include_hidden_topics && topic_is_hidden(topic_name)) {
RCUTILS_LOG_WARN_ONCE_NAMED(
ROSBAG2_TRANSPORT_PACKAGE_NAME,
Expand All @@ -197,23 +203,29 @@ bool TopicFilter::take_topic(
return false;
}

// Convert service event topic name to service name
auto service_name = rosbag2_cpp::service_event_topic_name_to_service_name(topic_name);

std::regex exclude_services_regex(record_options_.exclude_services);
if (!record_options_.exclude_services.empty() &&
std::regex_search(service_name, exclude_services_regex))
service_in_list(topic_name, record_options_.exclude_services))
{
return false;
}

// Convert service event topic name to service name
auto service_name = rosbag2_cpp::service_event_topic_name_to_service_name(topic_name);

std::regex include_regex(record_options_.regex);
if (!record_options_.all_services && // All takes precedence over regex
!record_options_.regex.empty() && // empty regex matches nothing, but should be ignored
!std::regex_search(service_name, include_regex))
{
return false;
}

if (!record_options_.exclude_regex.empty()) {
std::regex exclude_regex(record_options_.exclude_regex);
if (std::regex_search(service_name, exclude_regex)) {
return false;
}
}
}

if (!allow_unknown_types_ && !type_is_known(topic_name, topic_type)) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -187,7 +187,7 @@ TEST_F(RecordIntegrationTestFixture, test_keyboard_controls)
auto keyboard_handler = std::make_shared<MockKeyboardHandler>();

rosbag2_transport::RecordOptions record_options =
{true, false, false, {}, {}, "rmw_format", 100ms};
{true, false, false, {}, {}, {}, {}, "rmw_format", 100ms};
record_options.start_paused = true;

auto recorder = std::make_shared<Recorder>(
Expand Down
16 changes: 8 additions & 8 deletions rosbag2_transport/test/rosbag2_transport/test_record.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are
pub_manager.setup_publisher(string_topic, string_message, 2);

rosbag2_transport::RecordOptions record_options =
{false, false, false, {string_topic, array_topic}, {}, "rmw_format", 50ms};
{false, false, false, {string_topic, array_topic}, {}, {}, {}, "rmw_format", 50ms};
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
recorder->record();
Expand Down Expand Up @@ -98,7 +98,7 @@ TEST_F(RecordIntegrationTestFixture, can_record_again_after_stop)
pub_manager.setup_publisher(string_topic, string_message, 2);

rosbag2_transport::RecordOptions record_options =
{false, false, false, {string_topic}, {}, "rmw_format", 50ms};
{false, false, false, {string_topic}, {}, {}, {}, "rmw_format", 50ms};
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
recorder->record();
Expand Down Expand Up @@ -150,7 +150,7 @@ TEST_F(RecordIntegrationTestFixture, qos_is_stored_in_metadata)
pub_manager.setup_publisher(topic, string_message, 2);

rosbag2_transport::RecordOptions record_options =
{false, false, false, {topic}, {}, "rmw_format", 100ms};
{false, false, false, {topic}, {}, {}, {}, "rmw_format", 100ms};
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
recorder->record();
Expand Down Expand Up @@ -214,7 +214,7 @@ TEST_F(RecordIntegrationTestFixture, records_sensor_data)
pub_manager.setup_publisher(topic, string_message, 2, rclcpp::SensorDataQoS());

rosbag2_transport::RecordOptions record_options =
{false, false, false, {topic}, {}, "rmw_format", 100ms};
{false, false, false, {topic}, {}, {}, {}, "rmw_format", 100ms};
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
recorder->record();
Expand Down Expand Up @@ -257,7 +257,7 @@ TEST_F(RecordIntegrationTestFixture, receives_latched_messages)
pub_manager.run_publishers();

rosbag2_transport::RecordOptions record_options =
{false, false, false, {topic}, {}, "rmw_format", 100ms};
{false, false, false, {topic}, {}, {}, {}, "rmw_format", 100ms};
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
recorder->record();
Expand Down Expand Up @@ -302,7 +302,7 @@ TEST_F(RecordIntegrationTestFixture, mixed_qos_subscribes) {
topic, profile_transient_local);

rosbag2_transport::RecordOptions record_options =
{false, false, false, {topic}, {}, "rmw_format", 100ms};
{false, false, false, {topic}, {}, {}, {}, "rmw_format", 100ms};
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
recorder->record();
Expand Down Expand Up @@ -349,7 +349,7 @@ TEST_F(RecordIntegrationTestFixture, duration_and_noncompatibility_policies_mixe
auto publisher_liveliness = create_pub(profile_liveliness);

rosbag2_transport::RecordOptions record_options =
{false, false, false, {topic}, {}, "rmw_format", 100ms};
{false, false, false, {topic}, {}, {}, {}, "rmw_format", 100ms};
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
recorder->record();
Expand Down Expand Up @@ -387,7 +387,7 @@ TEST_F(RecordIntegrationTestFixture, write_split_callback_is_called)
mock_writer.set_max_messages_per_file(5);

rosbag2_transport::RecordOptions record_options =
{false, false, false, {string_topic}, {}, "rmw_format", 100ms};
{false, false, false, {string_topic}, {}, {}, {}, "rmw_format", 100ms};
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
recorder->record();
Expand Down
7 changes: 3 additions & 4 deletions rosbag2_transport/test/rosbag2_transport/test_record_all.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are
pub_manager.setup_publisher(string_topic, string_message, 2);

rosbag2_transport::RecordOptions record_options =
{true, false, false, {}, {}, "rmw_format", 100ms};
{true, false, false, {}, {}, {}, {}, "rmw_format", 100ms};
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
recorder->record();
Expand Down Expand Up @@ -100,7 +100,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_services_a
"test_service_2");

rosbag2_transport::RecordOptions record_options =
{false, true, false, {}, {}, "rmw_format", 100ms};
{false, true, false, {}, {}, {}, {}, "rmw_format", 100ms};
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
recorder->record();
Expand Down Expand Up @@ -140,8 +140,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_a
pub_manager.setup_publisher(string_topic, string_message, 1);

rosbag2_transport::RecordOptions record_options =
{true, true, false, {}, {}, "rmw_format", 100ms};
record_options.exclude_topics = "rosout";
{true, true, false, {}, {}, {"rosout"}, {}, "rmw_format", 100ms};
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
recorder->record();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_two_topics_ignore_l
pub_manager.setup_publisher(string_topic, string_message, 2);

rosbag2_transport::RecordOptions record_options =
{true, false, false, {}, {}, "rmw_format", 100ms};
{true, false, false, {}, {}, {}, {}, "rmw_format", 100ms};
record_options.ignore_leaf_topics = true;
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ TEST_F(RecordIntegrationTestFixture, record_all_include_unpublished_false_ignore
string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {});

rosbag2_transport::RecordOptions record_options =
{true, false, false, {}, {}, "rmw_format", 100ms};
{true, false, false, {}, {}, {}, {}, "rmw_format", 100ms};
record_options.include_unpublished_topics = false;
auto recorder = std::make_shared<MockRecorder>(writer_, storage_options_, record_options);
recorder->record();
Expand All @@ -53,7 +53,7 @@ TEST_F(RecordIntegrationTestFixture, record_all_include_unpublished_true_include
string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {});

rosbag2_transport::RecordOptions record_options =
{true, false, false, {}, {}, "rmw_format", 100ms};
{true, false, false, {}, {}, {}, {}, "rmw_format", 100ms};
record_options.include_unpublished_topics = true;
auto recorder = std::make_shared<MockRecorder>(writer_, storage_options_, record_options);
recorder->record();
Expand All @@ -73,7 +73,7 @@ TEST_F(
string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {});

rosbag2_transport::RecordOptions record_options =
{true, false, false, {}, {}, "rmw_format", 100ms};
{true, false, false, {}, {}, {}, {}, "rmw_format", 100ms};
record_options.include_unpublished_topics = false;
auto recorder = std::make_shared<MockRecorder>(writer_, storage_options_, record_options);
recorder->record();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ TEST_F(RecordIntegrationTestFixture, record_all_without_discovery_ignores_later_
string_message->string_value = "Hello World";

rosbag2_transport::RecordOptions record_options =
{true, false, true, {}, {}, "rmw_format", 100ms};
{true, false, true, {}, {}, {}, {}, "rmw_format", 100ms};
auto recorder = std::make_shared<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options);
recorder->record();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@ TEST_F(RecordIntegrationTestFixture, record_all_with_sim_time)

rosbag2_transport::RecordOptions record_options =
{
false, false, false, {string_topic, clock_topic}, {}, "rmw_format", 100ms
false, false, false, {string_topic, clock_topic}, {}, {}, {}, "rmw_format", 100ms
};
record_options.use_sim_time = true;
auto recorder = std::make_shared<MockRecorder>(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,12 @@ TEST(record_options, test_yaml_serialization)
original.is_discovery_disabled = true;
original.topics = {"topic", "other_topic"};
original.services = {"service", "other_service"};
original.exclude_topics = {"exclude_topic1", "exclude_topic2"};
original.exclude_services = {"exclude_service1", "exclude_service2"};
original.rmw_serialization_format = "cdr";
original.topic_polling_interval = std::chrono::milliseconds{200};
original.regex = "[xyz]/topic";
original.exclude_topics = "*";
original.exclude_services = "*";
original.exclude_regex = "[x]/topic";
original.node_prefix = "prefix";
original.compression_mode = "stream";
original.compression_format = "h264";
Expand All @@ -53,6 +54,8 @@ TEST(record_options, test_yaml_serialization)
CHECK(is_discovery_disabled);
CHECK(topics);
CHECK(services);
CHECK(exclude_topics);
CHECK(exclude_services);
CHECK(rmw_serialization_format);
#undef CMP
}
Loading

0 comments on commit b6f732e

Please sign in to comment.