i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 12:07:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix the crash caused by publishScanToPoint

Browse Source
This commit is contained in:
jj
2025-07-08 14:29:29 +08:00
parent 8119b233d8
commit 330198c0a6
2 changed files with 5 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/launch/lidar.launch.py
+1 -1
View File
@@ -59,7 +59,7 @@ def generate_launch_description():
DeclareLaunchArgument('publish_tf', default_value='true'), DeclareLaunchArgument('publish_tf', default_value='true'),
DeclareLaunchArgument('tf_publish_rate', default_value='0.0'), DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
DeclareLaunchArgument('lidar_format', default_value='ANY'),#LIDAR_POINT, LIDAR_SPHERE_POINT, LIDAR_SCAN DeclareLaunchArgument('lidar_format', default_value='ANY'),#LIDAR_POINT, LIDAR_SPHERE_POINT, LIDAR_SCAN
DeclareLaunchArgument('lidar_rate', default_value='30'), DeclareLaunchArgument('lidar_rate', default_value='20'),
DeclareLaunchArgument('enable_scan_to_point', default_value='false'), DeclareLaunchArgument('enable_scan_to_point', default_value='false'),
DeclareLaunchArgument('repetitive_scan_mode', default_value='-1'), DeclareLaunchArgument('repetitive_scan_mode', default_value='-1'),
DeclareLaunchArgument('filter_level', default_value='-1'), DeclareLaunchArgument('filter_level', default_value='-1'),
orbbec_camera/src/ob_lidar_node.cpp
+4 -10
View File
@@ -326,11 +326,11 @@ void OBLidarNode::setupPublishers() {
if (use_intra_process_) { if (use_intra_process_) {
point_cloud_qos_profile = rmw_qos_profile_default; point_cloud_qos_profile = rmw_qos_profile_default;
} }
if (format_[LIDAR] == OB_FORMAT_LIDAR_SCAN) { if (!enable_scan_to_point_ && format_[LIDAR] == OB_FORMAT_LIDAR_SCAN) {
scan_pub_ = node_->create_publisher<sensor_msgs::msg::LaserScan>( scan_pub_ = node_->create_publisher<sensor_msgs::msg::LaserScan>(
"scan/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile), "scan/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
point_cloud_qos_profile)); point_cloud_qos_profile));
} else if (format_[LIDAR] == OB_FORMAT_LIDAR_POINT || } else if (enable_scan_to_point_ || format_[LIDAR] == OB_FORMAT_LIDAR_POINT ||
format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT) { format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT) {
point_cloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>( point_cloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>(
"cloud/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile), "cloud/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
@@ -504,14 +504,10 @@ void OBLidarNode::publishScanToPoint(std::shared_ptr<ob::FrameSet> frame_set) {
for (size_t i = 0; i < scan_count; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_reflectivity) { for (size_t i = 0; i < scan_count; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_reflectivity) {
double rad = 0.7853981852531433 + 0.0026179938577115536 * i; double rad = 0.7853981852531433 + 0.0026179938577115536 * i;
*iter_x = static_cast<float>(scans_data[i].distance * cos(rad) / 1000.0); *iter_x = static_cast<float>(scans_data[i].distance * cos(rad) / 1000.0);
*iter_y = static_cast<float>(scans_data[i].distance * sin(rad) / -1000.0); *iter_y = static_cast<float>(scans_data[i].distance * sin(rad) / 1000.0);
*iter_z = static_cast<float>(0.0); *iter_z = static_cast<float>(0.0);
*iter_reflectivity = static_cast<uint8_t>(scans_data[i].intensity); *iter_reflectivity = static_cast<uint8_t>(scans_data[i].intensity);
// RCLCPP_INFO_STREAM(logger_, "*iter_x:"<<*iter_x);
} }
RCLCPP_INFO(logger_, "point_step: %u", point_cloud_msg->point_step);
RCLCPP_INFO(logger_, "row_step: %u", point_cloud_msg->row_step);
RCLCPP_INFO(logger_, "data size: %zu", point_cloud_msg->data.size());
*point_cloud_msg = filterPointCloud(*point_cloud_msg); *point_cloud_msg = filterPointCloud(*point_cloud_msg);
point_cloud_pub_->publish(std::move(point_cloud_msg)); point_cloud_pub_->publish(std::move(point_cloud_msg));
} }
@@ -707,12 +703,10 @@ sensor_msgs::msg::PointCloud2 OBLidarNode::filterPointCloud(
sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud, "x"); sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud, "y"); sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud, "z"); sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_reflectivity(point_cloud, "reflectivity");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_tag(point_cloud, "tag");
// Process each point // Process each point
for (size_t i = 0; i < point_cloud.height * point_cloud.width; for (size_t i = 0; i < point_cloud.height * point_cloud.width;
++i, ++iter_x, ++iter_y, ++iter_z, ++iter_reflectivity, ++iter_tag) { ++i, ++iter_x, ++iter_y, ++iter_z) {
float x = *iter_x; float x = *iter_x;
float y = *iter_y; float y = *iter_y;
float z = *iter_z; float z = *iter_z;