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

Fix the problem that angle_increment caused abnormal scan topic data

Browse Source
This commit is contained in:
jj
2025-07-08 15:01:28 +08:00
parent 330198c0a6
commit f622ea57d5
4 changed files with 23 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_lidar_node.h
+1
View File
@@ -250,6 +250,7 @@ class OBLidarNode {
int repetitive_scan_mode_ = -1; int repetitive_scan_mode_ = -1;
int filter_level_ = -1; int filter_level_ = -1;
float vertical_fov_ = -1; float vertical_fov_ = -1;
double angle_increment_ = 0.0;
}; };
} // namespace orbbec_lidar } // namespace orbbec_lidar
orbbec_camera/include/orbbec_camera/utils.h
+2
View File
@@ -197,6 +197,8 @@ cv::Mat undistortImage(const cv::Mat& image, const OBCameraIntrinsic& intrinsic,
std::string getDistortionModels(OBCameraDistortion distortion); std::string getDistortionModels(OBCameraDistortion distortion);
double getScanAngleIncrement(OBLiDARScanRate fps);
double deg2rad(double deg); double deg2rad(double deg);
double rad2deg(double rad); double rad2deg(double rad);
orbbec_camera/src/ob_lidar_node.cpp
+4 -1
View File
@@ -439,6 +439,9 @@ void OBLidarNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set)
void OBLidarNode::publishScan(std::shared_ptr<ob::FrameSet> frame_set) { void OBLidarNode::publishScan(std::shared_ptr<ob::FrameSet> frame_set) {
(void)frame_set; (void)frame_set;
if (angle_increment_ == 0.0) {
angle_increment_ = getScanAngleIncrement(rate_[LIDAR]);
}
if (frame_set == nullptr) { if (frame_set == nullptr) {
return; return;
} }
@@ -453,7 +456,7 @@ void OBLidarNode::publishScan(std::shared_ptr<ob::FrameSet> frame_set) {
scan_msg->header.frame_id = frame_id_[LIDAR]; scan_msg->header.frame_id = frame_id_[LIDAR];
scan_msg->angle_min = 0.7853981852531433; scan_msg->angle_min = 0.7853981852531433;
scan_msg->angle_max = 5.495169162750244; scan_msg->angle_max = 5.495169162750244;
scan_msg->angle_increment = 0.0026179938577115536; scan_msg->angle_increment = angle_increment_;
scan_msg->time_increment = 1.0 / rate_int_[LIDAR] / scan_count; scan_msg->time_increment = 1.0 / rate_int_[LIDAR] / scan_count;
scan_msg->scan_time = 1.0 / rate_int_[LIDAR]; scan_msg->scan_time = 1.0 / rate_int_[LIDAR];
scan_msg->range_min = min_range_; scan_msg->range_min = min_range_;
orbbec_camera/src/utils.cpp
+16
View File
@@ -961,6 +961,22 @@ std::string getDistortionModels(OBCameraDistortion distortion) {
return sensor_msgs::distortion_models::PLUMB_BOB; return sensor_msgs::distortion_models::PLUMB_BOB;
} }
} }
double getScanAngleIncrement(OBLiDARScanRate fps) {
switch (fps) {
case OB_LIDAR_SCAN_15HZ:
return deg2rad(0.075);
case OB_LIDAR_SCAN_20HZ:
return deg2rad(0.1);
case OB_LIDAR_SCAN_25HZ:
return deg2rad(0.125);
case OB_LIDAR_SCAN_30HZ:
return deg2rad(0.15);
case OB_LIDAR_SCAN_40HZ:
return deg2rad(0.2);
default:
return deg2rad(0.1);
}
}
double deg2rad(double deg) { return deg * M_PI / 180.0; } double deg2rad(double deg) { return deg * M_PI / 180.0; }