Skip to content

Commit

Permalink
Record option declaration
Browse files Browse the repository at this point in the history
Dependency fixes

Fix

Cpplint

Fix
  • Loading branch information
roncapat committed Jul 17, 2023
1 parent 7f467a9 commit 8332f70
Show file tree
Hide file tree
Showing 4 changed files with 133 additions and 1 deletion.
4 changes: 4 additions & 0 deletions rosbag2_storage/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,8 @@ endif()

find_package(ament_cmake REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rcl_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rcpputils REQUIRED)
find_package(rcutils REQUIRED)
find_package(yaml_cpp_vendor REQUIRED)
Expand All @@ -45,6 +47,8 @@ target_link_libraries(${PROJECT_NAME}
pluginlib::pluginlib
rcpputils::rcpputils
rcutils::rcutils
rclcpp::rclcpp
${rcl_interfaces_TARGETS}
yaml-cpp
)

Expand Down
2 changes: 2 additions & 0 deletions rosbag2_storage/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@

<depend>pluginlib</depend>
<depend>rcpputils</depend>
<depend>rcl_interfaces</depend>
<depend>rclcpp</depend>
<depend>rcutils</depend>
<depend>yaml_cpp_vendor</depend>

Expand Down
3 changes: 2 additions & 1 deletion rosbag2_transport/include/rosbag2_transport/play_options.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,8 @@ void declare_play_options_rw_params(std::shared_ptr<rclcpp::Node> nh, PlayOption

// TODO(roncapat): but I think it's worth to use classic CLI/launchfile remap instead
// po.topic_remapping_options =
// nh->declare_parameter<std::vector<std::string>>("play.topic_remapping_options", empty_str_list);
// nh->declare_parameter<std::vector<std::string>>(
// "play.topic_remapping_options", empty_str_list);

po.clock_publish_frequency = nh->declare_parameter<double>(
"play.clock_publish_frequency",
Expand Down
125 changes: 125 additions & 0 deletions rosbag2_transport/include/rosbag2_transport/record_options.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@
#define ROSBAG2_TRANSPORT__RECORD_OPTIONS_HPP_

#include <chrono>
#include <memory>
#include <limits>
#include <string>
#include <unordered_map>
#include <vector>
Expand Down Expand Up @@ -50,6 +52,129 @@ struct RecordOptions
bool use_sim_time = false;
};

namespace
{
rcl_interfaces::msg::ParameterDescriptor int_param_description(
std::string description, int64_t min,
int64_t max)
{
rcl_interfaces::msg::ParameterDescriptor d{};
rcl_interfaces::msg::IntegerRange r{};
d.description = description;
r.from_value = min;
r.to_value = max;
d.integer_range.push_back(r);
return d;
}

rcl_interfaces::msg::ParameterDescriptor float_param_description(
std::string description, float min,
float max)
{
rcl_interfaces::msg::ParameterDescriptor d{};
rcl_interfaces::msg::FloatingPointRange r{};
d.description = description;
r.from_value = min;
r.to_value = max;
d.floating_point_range.push_back(r);
return d;
}
} // namespace

void declare_record_options_rw_params(std::shared_ptr<rclcpp::Node> nh, RecordOptions & ro)
{
static const std::vector<std::string> empty_str_list;

ro.all = nh->declare_parameter<bool>(
"record.all",
false);

ro.is_discovery_disabled = nh->declare_parameter<bool>(
"record.is_discovery_disabled",
false);

ro.topics = nh->declare_parameter<std::vector<std::string>>(
"record.topics",
empty_str_list);

ro.rmw_serialization_format = nh->declare_parameter<std::string>(
"record.rmw_serialization_format",
"");

auto desc_tpi = int_param_description(
"Topic polling interval (ms)",
1,
std::numeric_limits<int64_t>::max());
auto topic_polling_interval_ = nh->declare_parameter<int64_t>(
"record.topic_polling_interval",
100,
desc_tpi);
ro.topic_polling_interval = std::chrono::milliseconds{topic_polling_interval_};

ro.regex = nh->declare_parameter<std::string>(
"record.regex",
"");

ro.exclude = nh->declare_parameter<std::string>(
"record.exclude",
"");

ro.node_prefix = nh->declare_parameter<std::string>(
"record.node_prefix",
"");

ro.compression_mode = nh->declare_parameter<std::string>(
"record.compression_mode",
"");

ro.compression_format = nh->declare_parameter<std::string>(
"record.compression_format",
"");

auto desc_cqs = int_param_description(
"Compression queue size (messages)",
1,
std::numeric_limits<int64_t>::max());
auto compression_queue_size_ = nh->declare_parameter<int64_t>(
"record.compression_queue_size",
1,
desc_cqs);
ro.compression_queue_size = std::static_cast<uint64_t>(compression_queue_size_);

auto desc_cts = int_param_description(
"Compression threads",
0,
std::numeric_limits<int64_t>::max());
auto compression_threads_ = nh->declare_parameter<int64_t>(
"record.compression_threads",
0,
desc_cts);
ro.compression_threads = std::static_cast<uint64_t>(compression_threads_);

// TODO(roncapat)
// std::unordered_map<std::string, rclcpp::QoS> topic_qos_profile_overrides{};

ro.include_hidden_topics = nh->declare_parameter<bool>(
"record.include_hidden_topics",
false);

ro.include_unpublished_topics = nh->declare_parameter<bool>(
"record.include_unpublished_topics",
false);

ro.ignore_leaf_topics = nh->declare_parameter<bool>(
"record.ignore_leaf_topics",
false);

ro.start_paused = nh->declare_parameter<bool>(
"record.start_paused",
false);

ro.use_sim_time = nh->declare_parameter<bool>(
"record.use_sim_time",
false);
}

} // namespace rosbag2_transport

namespace YAML
Expand Down

0 comments on commit 8332f70

Please sign in to comment.