Skip to content

Commit

Permalink
refactor code
Browse files Browse the repository at this point in the history 
Signed-off-by: ismetatabay <[email protected]>
  • Loading branch information
@ismetatabay
ismetatabay committed Aug 21, 2023
1 parent cc65ca4 commit c749f99
Show file tree
Hide file tree
Showing 3 changed files with 163 additions and 137 deletions.
2 changes: 1 addition & 1 deletion tools/autoware_bag_recorder/config/autoware_bag_recorder.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
common:
maximum_record_time: 1800 # seconds
bag_time: 100 # seconds
enable_only_auto_mode_recording: true
enable_only_auto_mode_recording: false
record_all_topic_in_a_bag: true
number_of_maximum_bags: 5
path: "bags/" # Path to bag file for saving
Expand Down
17 changes: 16 additions & 1 deletion tools/autoware_bag_recorder/include/autoware_bag_recorder.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,21 @@ class AutowareBagRecorderNode : public rclcpp::Node
AutowareBagRecorderNode(const std::string & node_name, const rclcpp::NodeOptions & options);

private:
void initialize_parameters();
void setup_module_sections();
void setup_single_module(
const std::string & module_param, std::vector<std::string> & topics,
const std::string & section_name);
void record_all_topics_in_a_bag();
void check_and_remove_files_at_init();
void initialize_bag_files_for_topics();
void start_topic_search();
void start_status_control();
void check_disk_space();
void check_record_time(
const std::chrono::time_point<std::chrono::system_clock> & start_record_time);
void check_bag_time(std::chrono::time_point<std::chrono::system_clock> & start_bag_time);
void check_auto_mode();
static std::string get_timestamp();
rclcpp::QoS get_qos_profile_of_topic(const std::string & topic_name);
void search_topic(ModuleSection & section);
Expand Down Expand Up @@ -122,7 +137,7 @@ class AutowareBagRecorderNode : public rclcpp::Node
rclcpp::Node::SharedPtr node_;

std::shared_ptr<rclcpp::SerializedMessage> serialized_msg_ptr_;
tier4_control_msgs::msg::GateMode::ConstSharedPtr gate_mode_cmd_ptr;
tier4_control_msgs::msg::GateMode::ConstSharedPtr gate_mode_msg_ptr;

std::mutex writer_mutex_;
};
Expand Down
281 changes: 146 additions & 135 deletions tools/autoware_bag_recorder/src/autoware_bag_recorder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,30 @@ namespace autoware_bag_recorder
AutowareBagRecorderNode::AutowareBagRecorderNode(
const std::string & node_name, const rclcpp::NodeOptions & node_options)
: rclcpp::Node(node_name, node_options), node_(this)
{
// create gate mode subscription
gate_mode_sub_ = create_subscription<tier4_control_msgs::msg::GateMode>(
"/control/current_gate_mode", 1,
std::bind(&AutowareBagRecorderNode::gate_mode_cmd_callback, this, std::placeholders::_1));

// initialize common parameters
initialize_parameters();

// initialize module sections
setup_module_sections();

// Check the files at initialization
check_and_remove_files_at_init();

// check recording all topics in a single bag file is enabled
if (record_all_topic_in_a_bag_) {
record_all_topics_in_a_bag();
}

run();
}

void AutowareBagRecorderNode::initialize_parameters()
{
// common params declarations
bag_path_ = declare_parameter<std::string>("common.path");
Expand All @@ -36,110 +60,53 @@ AutowareBagRecorderNode::AutowareBagRecorderNode(

is_writing_ = false;
remaining_topic_num_ = 0;
}

// api
record_api_topics_ = declare_parameter<bool>("api_modules.record_api");
if (record_api_topics_ || record_all_topic_in_a_bag_) {
api_topics_ = declare_parameter<std::vector<std::string>>("api_modules.api_topics");
section_factory(api_topics_, bag_path_ + "api");
}

// autoware
record_autoware_topics_ = declare_parameter<bool>("autoware_modules.record_autoware");
if (record_autoware_topics_ || record_all_topic_in_a_bag_) {
autoware_topics_ =
declare_parameter<std::vector<std::string>>("autoware_modules.autoware_topics");
section_factory(autoware_topics_, bag_path_ + "autoware");
}

// control
record_control_topics_ = declare_parameter<bool>("control_modules.record_control");
if (record_control_topics_ || record_all_topic_in_a_bag_) {
control_topics_ = declare_parameter<std::vector<std::string>>("control_modules.control_topics");
section_factory(control_topics_, bag_path_ + "control");
}

// external
record_external_topics_ = declare_parameter<bool>("external_modules.record_external");
if (record_external_topics_ || record_all_topic_in_a_bag_) {
external_topics_ =
declare_parameter<std::vector<std::string>>("external_modules.external_topics");
section_factory(external_topics_, bag_path_ + "external");
}

// localization
record_localization_topics_ = declare_parameter<bool>("localization_modules.record_localization");
if (record_localization_topics_ || record_all_topic_in_a_bag_) {
localization_topics_ =
declare_parameter<std::vector<std::string>>("localization_modules.localization_topics");
section_factory(localization_topics_, bag_path_ + "localization");
}

// map
record_map_topics_ = declare_parameter<bool>("map_modules.record_map");
if (record_map_topics_ || record_all_topic_in_a_bag_) {
map_topics_ = declare_parameter<std::vector<std::string>>("map_modules.map_topics");
section_factory(map_topics_, bag_path_ + "map");
}

// perception
record_perception_topics_ = declare_parameter<bool>("perception_modules.record_perception");
if (record_perception_topics_ || record_all_topic_in_a_bag_) {
perception_topics_ =
declare_parameter<std::vector<std::string>>("perception_modules.perception_topics");
section_factory(perception_topics_, bag_path_ + "perception");
}

// planning
record_planning_topics_ = declare_parameter<bool>("planning_modules.record_planning");
if (record_planning_topics_ || record_all_topic_in_a_bag_) {
planning_topics_ =
declare_parameter<std::vector<std::string>>("planning_modules.planning_topics");
section_factory(planning_topics_, bag_path_ + "planning");
}

// sensing
record_sensing_topics_ = declare_parameter<bool>("sensing_modules.record_sensing");
if (record_sensing_topics_ || record_all_topic_in_a_bag_) {
sensing_topics_ = declare_parameter<std::vector<std::string>>("sensing_modules.sensing_topics");
section_factory(sensing_topics_, bag_path_ + "sensing");
}

// system
record_system_topics_ = declare_parameter<bool>("system_modules.record_system");
if (record_system_topics_ || record_all_topic_in_a_bag_) {
system_topics_ = declare_parameter<std::vector<std::string>>("system_modules.system_topics");
section_factory(system_topics_, bag_path_ + "system");
}
void AutowareBagRecorderNode::setup_module_sections()
{
setup_single_module("api_modules", api_topics_, "api");
setup_single_module("autoware_modules", autoware_topics_, "autoware");
setup_single_module("control_modules", control_topics_, "control");
setup_single_module("external_modules", external_topics_, "external");
setup_single_module("localization_modules", localization_topics_, "localization");
setup_single_module("map_modules", map_topics_, "map");
setup_single_module("perception_modules", perception_topics_, "perception");
setup_single_module("planning_modules", planning_topics_, "planning");
setup_single_module("sensing_modules", sensing_topics_, "sensing");
setup_single_module("system_modules", system_topics_, "system");
setup_single_module("vehicle_modules", vehicle_topics_, "vehicle");
}

// vehicle
record_vehicle_topics_ = declare_parameter<bool>("vehicle_modules.record_vehicle");
if (record_vehicle_topics_ || record_all_topic_in_a_bag_) {
vehicle_topics_ = declare_parameter<std::vector<std::string>>("vehicle_modules.vehicle_topics");
section_factory(vehicle_topics_, bag_path_ + "vehicle");
void AutowareBagRecorderNode::setup_single_module(
const std::string & module_param, std::vector<std::string> & topics,
const std::string & section_name)
{
bool record_module_topics = declare_parameter<bool>(module_param + ".record_" + section_name);
if (record_module_topics || record_all_topic_in_a_bag_) {
topics =
declare_parameter<std::vector<std::string>>(module_param + "." + section_name + "_topics");
section_factory(topics, bag_path_ + section_name);
}
}

gate_mode_sub_ = create_subscription<tier4_control_msgs::msg::GateMode>(
"/control/current_gate_mode", 1,
std::bind(&AutowareBagRecorderNode::gate_mode_cmd_callback, this, std::placeholders::_1));

if (record_all_topic_in_a_bag_) {
ModuleSection all_topics;
std::vector<std::string> all_topic_names;
for (const auto & section : module_sections_) {
for (const auto & topic : section.topic_names) {
all_topic_names.push_back(topic);
}
void AutowareBagRecorderNode::record_all_topics_in_a_bag()
{
ModuleSection all_topics;
std::vector<std::string> all_topic_names;
for (const auto & section : module_sections_) {
for (const auto & topic : section.topic_names) {
all_topic_names.push_back(topic);
}
module_sections_ = std::vector<ModuleSection>();
section_factory(all_topic_names, bag_path_ + "all");
}
module_sections_ = std::vector<ModuleSection>();
section_factory(all_topic_names, bag_path_ + "all");
}

void AutowareBagRecorderNode::check_and_remove_files_at_init()
{
for (auto & section : module_sections_) {
remove_remainder_bags_in_folder(section);
}

run();
}

std::string AutowareBagRecorderNode::get_timestamp()
Expand Down Expand Up @@ -224,10 +191,10 @@ void AutowareBagRecorderNode::generic_subscription_callback(
const std::shared_ptr<rclcpp::SerializedMessage const> & msg, const std::string & topic_name,
autoware_bag_recorder::ModuleSection & section)
{
if (gate_mode_cmd_ptr) {
if (gate_mode_msg_ptr) {
if (
!enable_only_auto_mode_recording_ ||
(gate_mode_cmd_ptr->data == tier4_control_msgs::msg::GateMode::AUTO && is_writing_)) {
(gate_mode_msg_ptr->data == tier4_control_msgs::msg::GateMode::AUTO && is_writing_)) {
auto serialized_bag_msg = std::make_shared<rosbag2_storage::SerializedBagMessage>();
serialized_bag_msg->serialized_data = std::make_shared<rcutils_uint8_array_t>();
serialized_bag_msg->topic_name = topic_name;
Expand Down Expand Up @@ -335,19 +302,22 @@ void AutowareBagRecorderNode::free_disk_space_for_continue(
void AutowareBagRecorderNode::gate_mode_cmd_callback(
const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg)
{
gate_mode_cmd_ptr = msg; // AUTO = 1, EXTERNAL = 0
if (gate_mode_cmd_ptr->data != tier4_control_msgs::msg::GateMode::AUTO) {
gate_mode_msg_ptr = msg; // AUTO = 1, EXTERNAL = 0
if (gate_mode_msg_ptr->data != tier4_control_msgs::msg::GateMode::AUTO) {
is_writing_ = false;
}
}

void AutowareBagRecorderNode::run()
void AutowareBagRecorderNode::initialize_bag_files_for_topics()
{
for (auto & section : module_sections_) {
create_bag_file(section.bag_writer, section.folder_path + "/rosbag2_" + get_timestamp());
remaining_topic_num_ = remaining_topic_num_ + static_cast<int>(section.topic_names.size());
}
}

void AutowareBagRecorderNode::start_topic_search()
{
std::thread([this]() {
rclcpp::Rate rate(100);
// if all topics are not subscribed, then continue checking
Expand All @@ -358,57 +328,98 @@ void AutowareBagRecorderNode::run()
rate.sleep();
}
}).detach();
}

void AutowareBagRecorderNode::check_disk_space()
{
// check available disk space, if current disk space is smaller than threshold,
// then shutdown node
if (static_cast<int>(get_root_disk_space()) < disk_space_threshold_) {
RCLCPP_WARN(
this->get_logger(), "Available Disk Space is: %lf under the threshold.",
get_root_disk_space());
if (disk_space_action_mode_ == "remove") {
for (auto & section : module_sections_) {
free_disk_space_for_continue(section);
}
}
rclcpp::shutdown();
}
}

void AutowareBagRecorderNode::check_record_time(
const std::chrono::time_point<std::chrono::system_clock> & start_record_time)
{
// check record time, if record time is exceeded then shutdown node
if (
(std::chrono::system_clock::now() - start_record_time) >
std::chrono::seconds(maximum_record_time_)) {
RCLCPP_WARN(this->get_logger(), "The maximum record time is reached.");
rclcpp::shutdown();
}
}

void AutowareBagRecorderNode::check_bag_time(
std::chrono::time_point<std::chrono::system_clock> & start_bag_time)
{
// check record time, if bag time is exceeded then create new bag file
if (
((std::chrono::system_clock::now() - start_bag_time) >= std::chrono::seconds(bag_time_)) &&
!enable_only_auto_mode_recording_) {
start_bag_time = std::chrono::system_clock::now();
bag_file_handler();
}
}

void AutowareBagRecorderNode::check_auto_mode()
{
if (gate_mode_msg_ptr) {
if (
enable_only_auto_mode_recording_ && !is_writing_ &&
gate_mode_msg_ptr->data == tier4_control_msgs::msg::GateMode::AUTO) {
is_writing_ = true;
bag_file_handler();
}
} else {
RCLCPP_WARN(get_logger(), "The current gate mode not received!");
}
}

void AutowareBagRecorderNode::start_status_control()
{
std::thread([this]() {
rclcpp::Rate rate(10);

auto start_record_time = std::chrono::system_clock::now();
auto start_bag_time = std::chrono::system_clock::now();

while (rclcpp::ok()) {
// check available disk space, if current disk space is smaller than threshold,
// then shutdown node
if (static_cast<int>(get_root_disk_space()) < disk_space_threshold_) {
RCLCPP_WARN(
this->get_logger(), "Available Disk Space is: %lf under the threshold.",
get_root_disk_space());
if (disk_space_action_mode_ == "remove") {
for (auto & section : module_sections_) {
free_disk_space_for_continue(section);
}
}
rclcpp::shutdown();
}
// check available disk space
check_disk_space();

// check record time, if record time is exceeded then shutdown node
if (
(std::chrono::system_clock::now() - start_record_time) >
std::chrono::seconds(maximum_record_time_)) {
RCLCPP_WARN(this->get_logger(), "The maximum record time is reached.");
rclcpp::shutdown();
}
// check record time limit
check_record_time(start_record_time);

if (
((std::chrono::system_clock::now() - start_bag_time) >= std::chrono::seconds(bag_time_)) &&
!enable_only_auto_mode_recording_) {
start_bag_time = std::chrono::system_clock::now();
bag_file_handler();
}
// check bag time limit
check_record_time(start_bag_time);

if (gate_mode_cmd_ptr) {
if (
enable_only_auto_mode_recording_ && !is_writing_ &&
gate_mode_cmd_ptr->data == tier4_control_msgs::msg::GateMode::AUTO) {
is_writing_ = true;
bag_file_handler();
}
} else {
RCLCPP_WARN(get_logger(), "The current gate mode not received!");
}
// check autoware mode
check_auto_mode();

rate.sleep();
}
}).detach();
}
void AutowareBagRecorderNode::run()
{
// initialize bag files and topics according to the parameters
initialize_bag_files_for_topics();

// starting topic searching thread
start_topic_search();

// starting condition checking thread
start_status_control();
}

} // namespace autoware_bag_recorder

0 comments on commit c749f99

Please sign in to comment.