diff --git a/nebula_examples/CMakeLists.txt b/nebula_examples/CMakeLists.txt
index 311105e30..779987293 100644
--- a/nebula_examples/CMakeLists.txt
+++ b/nebula_examples/CMakeLists.txt
@@ -92,6 +92,12 @@ if(BUILD_TESTING)
ament_lint_auto_find_test_dependencies()
endif()
+install(TARGETS hesai_ros_offline_extract_pcd_node
+ hesai_ros_offline_extract_bag_pcd_node
+ velodyne_ros_offline_extract_bag_pcd_node
+ DESTINATION lib/${PROJECT_NAME}
+)
+
ament_auto_package(
INSTALL_TO_SHARE
launch
diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp
index 8b3e0520c..9427ef7fd 100644
--- a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp
+++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp
@@ -69,11 +69,17 @@ class HesaiRosOfflineExtractBag final : public rclcpp::Node
std::string storage_id_;
std::string out_path_;
std::string format_;
- std::string target_topic_;
+ std::string input_topic_;
std::string correction_file_path_;
+ std::string frame_id_;
+ std::string output_pointcloud_topic_;
int out_num_;
int skip_num_;
bool only_xyz_;
+
+ bool output_pcd_;
+ bool output_rosbag_;
+ bool forward_packets_to_rosbag_;
};
} // namespace nebula::ros
diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml
index acb604321..b9e308e89 100644
--- a/nebula_examples/launch/hesai_offline_bag_pcd.xml
+++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml
@@ -1,38 +1,41 @@
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
-
+
+
+
-
-
diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp
index 576a7f8bb..06e586aaa 100644
--- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp
+++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp
@@ -24,6 +24,8 @@
#include
+#include
+
#include
namespace nebula::ros
@@ -91,6 +93,7 @@ Status HesaiRosOfflineExtractBag::get_parameters(
nebula::drivers::return_mode_from_string_hesai(return_mode_, sensor_configuration.sensor_model);
this->declare_parameter("frame_id", "pandar", param_read_only());
sensor_configuration.frame_id = this->get_parameter("frame_id").as_string();
+ frame_id_ = sensor_configuration.frame_id;
{
rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only();
@@ -105,6 +108,19 @@ Status HesaiRosOfflineExtractBag::get_parameters(
descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]";
descriptor.floating_point_range = float_range(0, 360, 0.01);
sensor_configuration.cut_angle = declare_parameter("cut_angle", 0., param_read_only());
+ sensor_configuration.cloud_min_angle =
+ declare_parameter("cloud_min_angle", 0, param_read_only());
+ sensor_configuration.cloud_max_angle =
+ declare_parameter("cloud_max_angle", 360, param_read_only());
+ sensor_configuration.rotation_speed =
+ declare_parameter("rotation_speed", 600, param_read_only());
+ sensor_configuration.dual_return_distance_threshold =
+ declare_parameter("dual_return_distance_threshold", 0.1, param_read_only());
+ sensor_configuration.min_range = declare_parameter("min_range", 0.3, param_read_only());
+ sensor_configuration.max_range =
+ declare_parameter("max_range", 300.0, param_read_only());
+ sensor_configuration.packet_mtu_size =
+ declare_parameter("packet_mtu_size", 1500, param_read_only());
}
calibration_configuration.calibration_file =
@@ -121,7 +137,13 @@ Status HesaiRosOfflineExtractBag::get_parameters(
out_num_ = this->declare_parameter("out_num", 3, param_read_only());
skip_num_ = this->declare_parameter("skip_num", 3, param_read_only());
only_xyz_ = this->declare_parameter("only_xyz", false, param_read_only());
- target_topic_ = this->declare_parameter("target_topic", "", param_read_only());
+ input_topic_ = this->declare_parameter("input_topic", "", param_read_only());
+ output_pointcloud_topic_ =
+ this->declare_parameter("output_topic", "", param_read_only());
+ output_pcd_ = this->declare_parameter("output_pcd", false, param_read_only());
+ output_rosbag_ = this->declare_parameter("output_rosbag", true, param_read_only());
+ forward_packets_to_rosbag_ =
+ this->declare_parameter("forward_packets_to_rosbag", false, param_read_only());
if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) {
return Status::INVALID_SENSOR_MODEL;
@@ -170,18 +192,18 @@ Status HesaiRosOfflineExtractBag::read_bag()
std::cout << storage_id_ << std::endl;
std::cout << out_path_ << std::endl;
std::cout << format_ << std::endl;
- std::cout << target_topic_ << std::endl;
+ std::cout << input_topic_ << std::endl;
std::cout << out_num_ << std::endl;
std::cout << skip_num_ << std::endl;
std::cout << only_xyz_ << std::endl;
rcpputils::fs::path o_dir(out_path_);
- auto target_topic_name = target_topic_;
- if (target_topic_name.substr(0, 1) == "/") {
- target_topic_name = target_topic_name.substr(1);
+ auto input_topic_name = input_topic_;
+ if (input_topic_name.substr(0, 1) == "/") {
+ input_topic_name = input_topic_name.substr(1);
}
- target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_");
- o_dir = o_dir / rcpputils::fs::path(target_topic_name);
+ input_topic_name = std::regex_replace(input_topic_name, std::regex("/"), "_");
+ o_dir = o_dir / rcpputils::fs::path(input_topic_name);
if (rcpputils::fs::create_directories(o_dir)) {
std::cout << "created: " << o_dir << std::endl;
}
@@ -197,12 +219,13 @@ Status HesaiRosOfflineExtractBag::read_bag()
reader.open(storage_options, converter_options);
int cnt = 0;
int out_cnt = 0;
+ bool output_limit_reached = false;
while (reader.has_next()) {
auto bag_message = reader.read_next();
std::cout << "Found topic name " << bag_message->topic_name << std::endl;
- if (bag_message->topic_name != target_topic_) {
+ if (bag_message->topic_name != input_topic_) {
continue;
}
@@ -215,6 +238,31 @@ Status HesaiRosOfflineExtractBag::read_bag()
std::cout << "Found data in topic " << bag_message->topic_name << ": "
<< bag_message->time_stamp << std::endl;
+ // Initialize the bag writer if it is not initialized
+ if (!bag_writer) {
+ const rosbag2_storage::StorageOptions storage_options_w(
+ {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"});
+ const rosbag2_cpp::ConverterOptions converter_options_w(
+ {rmw_get_serialization_format(), rmw_get_serialization_format()});
+ bag_writer = std::make_unique();
+ bag_writer->open(storage_options_w, converter_options_w);
+ if (forward_packets_to_rosbag_) {
+ bag_writer->create_topic(
+ {bag_message->topic_name, "nebula_msgs/msg/NebulaPackets", rmw_get_serialization_format(),
+ ""});
+ }
+ if (output_rosbag_) {
+ bag_writer->create_topic(
+ {output_pointcloud_topic_, "sensor_msgs/msg/PointCloud2", rmw_get_serialization_format(),
+ ""});
+ }
+ }
+
+ // Forward the bag_message
+ if (forward_packets_to_rosbag_) {
+ bag_writer->write(bag_message);
+ }
+
nebula_msgs::msg::NebulaPackets nebula_msg;
nebula_msg.header = extracted_msg.header;
for (auto & pkt : extracted_msg.packets) {
@@ -233,35 +281,55 @@ Status HesaiRosOfflineExtractBag::read_bag()
auto fn = std::to_string(bag_message->time_stamp) + ".pcd";
- if (!bag_writer) {
- const rosbag2_storage::StorageOptions storage_options_w(
- {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"});
- const rosbag2_cpp::ConverterOptions converter_options_w(
- {rmw_get_serialization_format(), rmw_get_serialization_format()});
- bag_writer = std::make_unique();
- bag_writer->open(storage_options_w, converter_options_w);
- bag_writer->create_topic(
- {bag_message->topic_name, "nebula_msgs/msg/NebulaPackets", rmw_get_serialization_format(),
- ""});
- }
-
- bag_writer->write(bag_message);
cnt++;
if (skip_num_ < cnt) {
out_cnt++;
- if (only_xyz_) {
- pcl::PointCloud cloud_xyz;
- pcl::copyPointCloud(*pointcloud, cloud_xyz);
- pcd_writer.writeBinary((o_dir / fn).string(), cloud_xyz);
- } else {
- pcd_writer.writeBinary((o_dir / fn).string(), *pointcloud);
+
+ if (output_pcd_) {
+ if (only_xyz_) {
+ pcl::PointCloud cloud_xyz;
+ pcl::copyPointCloud(*pointcloud, cloud_xyz);
+ pcd_writer.writeBinary((o_dir / fn).string(), cloud_xyz);
+ } else {
+ pcd_writer.writeBinary((o_dir / fn).string(), *pointcloud);
+ }
+ }
+
+ if (output_rosbag_) {
+ // Create ROS Pointcloud from PCL pointcloud
+ sensor_msgs::msg::PointCloud2 cloud_msg;
+ pcl::toROSMsg(*pointcloud, cloud_msg);
+ cloud_msg.header = extracted_msg.header;
+ cloud_msg.header.frame_id = frame_id_;
+
+ // Create a serialized message for the pointcloud
+ rclcpp::SerializedMessage cloud_serialized_msg;
+ rclcpp::Serialization cloud_serialization;
+ cloud_serialization.serialize_message(&cloud_msg, &cloud_serialized_msg);
+
+ // Create a bag message for the pointcloud
+ auto cloud_bag_msg = std::make_shared();
+ cloud_bag_msg->topic_name = output_pointcloud_topic_;
+ cloud_bag_msg->time_stamp = bag_message->time_stamp;
+
+ // Create a new shared_ptr for the serialized data
+ cloud_bag_msg->serialized_data = std::make_shared(
+ cloud_serialized_msg.get_rcl_serialized_message());
+
+ // Write both messages to the bag
+ bag_writer->write(cloud_bag_msg);
}
}
- if (out_num_ <= out_cnt) {
+ if (out_num_ != 0 && out_num_ <= out_cnt) {
+ output_limit_reached = true;
break;
}
}
+
+ if (output_limit_reached) {
+ break;
+ }
}
return Status::OK;
}