Skip to content

Commit

Permalink
ros2GH-27 Provide polling frequency as option
Browse files Browse the repository at this point in the history
- make option available in RecordOptions
- set to 100ms for now
  • Loading branch information
Martin-Idel-SI authored and anhosi committed Nov 30, 2018
1 parent 1452bde commit eb06a3a
Show file tree
Hide file tree
Showing 6 changed files with 14 additions and 7 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#ifndef ROSBAG2_TRANSPORT__RECORD_OPTIONS_HPP_
#define ROSBAG2_TRANSPORT__RECORD_OPTIONS_HPP_

#include <chrono>
#include <string>
#include <vector>

Expand All @@ -26,6 +27,7 @@ struct RecordOptions
bool all;
std::vector<std::string> topics;
std::string rmw_serialization_format;
std::chrono::milliseconds topic_polling_frequency;
};

} // namespace rosbag2_transport
Expand Down
10 changes: 6 additions & 4 deletions rosbag2_transport/src/rosbag2_transport/recorder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,8 @@ Recorder::Recorder(std::shared_ptr<rosbag2::Writer> writer, std::shared_ptr<Rosb
void Recorder::record(const RecordOptions & record_options)
{
ROSBAG2_TRANSPORT_LOG_INFO("Setup complete. Listening for topics...");
auto discovery_future = launch_topics_discovery(record_options.topics);
auto discovery_future = launch_topics_discovery(
record_options.topic_polling_frequency, record_options.topics);
rclcpp::spin(node_);
discovery_future.wait();
subscriptions_.clear();
Expand Down Expand Up @@ -64,9 +65,10 @@ Recorder::create_subscription(
return subscription;
}

std::future<void> Recorder::launch_topics_discovery(std::vector<std::string> topics)
std::future<void> Recorder::launch_topics_discovery(
std::chrono::milliseconds topic_polling_frequency, std::vector<std::string> topics)
{
auto subscribe_to_topics = [this, topics] {
auto subscribe_to_topics = [this, topics, topic_polling_frequency] {
while (rclcpp::ok()) {
auto all_topics_and_types = topics.empty() ?
node_->get_all_topics_with_types() :
Expand All @@ -91,7 +93,7 @@ std::future<void> Recorder::launch_topics_discovery(std::vector<std::string> top
}
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
std::this_thread::sleep_for(topic_polling_frequency);
}
};

Expand Down
3 changes: 2 additions & 1 deletion rosbag2_transport/src/rosbag2_transport/recorder.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,8 @@ class Recorder
private:
std::shared_ptr<GenericSubscription> create_subscription(
const std::string & topic_name, const std::string & topic_type);
std::future<void> launch_topics_discovery(std::vector<std::string> topics = {});
std::future<void> launch_topics_discovery(
std::chrono::milliseconds topic_polling_frequency, std::vector<std::string> topics = {});

std::shared_ptr<rosbag2::Writer> writer_;
std::shared_ptr<Rosbag2Node> node_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
// limitations under the License.

#include <Python.h>
#include <chrono>
#include <string>
#include <vector>

Expand Down Expand Up @@ -48,6 +49,7 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject *
storage_options.uri = std::string(uri);
storage_options.storage_id = std::string(storage_id);
record_options.all = all;
record_options.topic_polling_frequency = std::chrono::milliseconds(100);

if (topics) {
PyObject * topic_iterator = PyObject_GetIter(topics);
Expand Down
2 changes: 1 addition & 1 deletion rosbag2_transport/test/rosbag2_transport/test_record.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are
string_message->string_value = "Hello World";
std::string string_topic = "/string_topic";

start_recording({false, {string_topic, array_topic}});
start_recording({false, {string_topic, array_topic}, 100ms});

pub_man_.add_publisher<test_msgs::msg::Primitives>(
string_topic, string_message, 2);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are
pub_man_.add_publisher<test_msgs::msg::Primitives>(string_topic, string_message, 2);
pub_man_.add_publisher<test_msgs::msg::StaticArrayPrimitives>(array_topic, array_message, 2);

start_recording({true, {}, ""});
start_recording({true, {}, "", 100ms});
run_publishers();
stop_recording();

Expand Down

0 comments on commit eb06a3a

Please sign in to comment.