mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-04 03:57:46 +08:00
Add publishPointCloud
This commit is contained in:
jj
@@ -170,333 +170,72 @@ class OBLidarNode {
|
||||
|
||||
void publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
|
||||
|
||||
void publishSpherePointCloud(std::shared_ptr<ob::FrameSet> frame_set);
|
||||
|
||||
uint64_t getFrameTimestampUs(const std::shared_ptr<ob::Frame>& frame);
|
||||
|
||||
void filterScan(sensor_msgs::msg::LaserScan& scan);
|
||||
|
||||
sensor_msgs::msg::PointCloud2 filterPointCloud(sensor_msgs::msg::PointCloud2& point_cloud) const;
|
||||
|
||||
void publishStaticTransforms();
|
||||
|
||||
void calcAndPublishStaticTransform();
|
||||
|
||||
void publishStaticTF(const rclcpp::Time& t, const tf2::Vector3& trans, const tf2::Quaternion& q,
|
||||
const std::string& from, const std::string& to);
|
||||
|
||||
void publishDynamicTransforms();
|
||||
|
||||
private:
|
||||
rclcpp::Node* node_ = nullptr;
|
||||
std::shared_ptr<ob::Device> device_ = nullptr;
|
||||
std::shared_ptr<Parameters> parameters_ = nullptr;
|
||||
std::condition_variable color_frame_queue_cv_;
|
||||
rclcpp::Logger logger_;
|
||||
std::atomic_bool is_running_{false};
|
||||
std::unique_ptr<ob::Pipeline> pipeline_ = nullptr;
|
||||
std::unique_ptr<ob::Pipeline> imuPipeline_ = nullptr;
|
||||
std::atomic_bool pipeline_started_{false};
|
||||
std::string camera_name_ = "camera";
|
||||
std::string accel_gyro_frame_id_ = "camera_accel_gyro_optical_frame";
|
||||
const std::string imu_frame_id_ = "camera_gyro_frame";
|
||||
std::shared_ptr<ob::Config> pipeline_config_ = nullptr;
|
||||
std::map<stream_index_pair, std::shared_ptr<ob::Sensor>> sensors_;
|
||||
std::map<stream_index_pair, ob_camera_intrinsic> stream_intrinsics_;
|
||||
std::map<stream_index_pair, sensor_msgs::msg::CameraInfo> camera_infos_;
|
||||
std::map<stream_index_pair, OBCameraParam> ob_camera_param_;
|
||||
std::map<stream_index_pair, OBExtrinsic> depth_to_other_extrinsics_;
|
||||
std::map<stream_index_pair, rclcpp::Publisher<orbbec_camera_msgs::msg::Extrinsics>::SharedPtr>
|
||||
depth_to_other_extrinsics_publishers_;
|
||||
std::map<stream_index_pair, rclcpp::Publisher<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
|
||||
metadata_publishers_;
|
||||
std::map<stream_index_pair, rclcpp::Publisher<orbbec_camera_msgs::msg::IMUInfo>::SharedPtr>
|
||||
imu_info_publishers_;
|
||||
std::map<stream_index_pair, int> width_;
|
||||
std::map<stream_index_pair, int> height_;
|
||||
std::map<stream_index_pair, int> fps_;
|
||||
std::map<stream_index_pair, std::string> optical_frame_id_;
|
||||
std::map<stream_index_pair, std::string> depth_aligned_frame_id_;
|
||||
std::string camera_link_frame_id_;
|
||||
bool depth_registration_ = false;
|
||||
std::map<stream_index_pair, std::string> image_qos_;
|
||||
std::map<stream_index_pair, std::string> camera_info_qos_;
|
||||
std::map<stream_index_pair, ob_format> format_;
|
||||
std::map<stream_index_pair, std::string> format_str_;
|
||||
std::map<stream_index_pair, int> image_format_;
|
||||
std::map<stream_index_pair, std::vector<std::shared_ptr<ob::LiDARStreamProfile>>>
|
||||
supported_profiles_;
|
||||
std::map<stream_index_pair, std::shared_ptr<ob::StreamProfile>> stream_profile_;
|
||||
stream_index_pair base_stream_ = LIDAR;
|
||||
std::map<stream_index_pair, uint32_t> seq_;
|
||||
std::map<stream_index_pair, cv::Mat> images_;
|
||||
std::map<stream_index_pair, std::string> encoding_;
|
||||
std::map<stream_index_pair, int> unit_step_size_;
|
||||
std::vector<int> compression_params_;
|
||||
ob::FormatConvertFilter format_convert_filter_;
|
||||
|
||||
std::map<stream_index_pair, bool> enable_stream_;
|
||||
std::map<stream_index_pair, bool> flip_stream_;
|
||||
std::map<stream_index_pair, bool> mirror_stream_;
|
||||
std::map<stream_index_pair, int> rotation_stream_;
|
||||
std::map<stream_index_pair, std::string> stream_name_;
|
||||
std::map<stream_index_pair, std::shared_ptr<image_publisher>> image_publishers_;
|
||||
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
|
||||
camera_info_publishers_;
|
||||
|
||||
std::map<stream_index_pair, rclcpp::Service<GetInt32>::SharedPtr> get_exposure_srv_;
|
||||
std::map<stream_index_pair, rclcpp::Service<SetInt32>::SharedPtr> set_exposure_srv_;
|
||||
std::map<stream_index_pair, rclcpp::Service<GetInt32>::SharedPtr> get_gain_srv_;
|
||||
std::map<stream_index_pair, rclcpp::Service<SetInt32>::SharedPtr> set_gain_srv_;
|
||||
std::map<stream_index_pair, rclcpp::Service<SetBool>::SharedPtr> toggle_sensor_srv_;
|
||||
std::map<stream_index_pair, rclcpp::Service<SetBool>::SharedPtr> set_mirror_srv_;
|
||||
std::map<stream_index_pair, rclcpp::Service<SetBool>::SharedPtr> set_flip_srv_;
|
||||
std::map<stream_index_pair, rclcpp::Service<SetInt32>::SharedPtr> set_rotation_srv_;
|
||||
rclcpp::Service<GetInt32>::SharedPtr get_white_balance_srv_;
|
||||
rclcpp::Service<SetInt32>::SharedPtr set_white_balance_srv_;
|
||||
rclcpp::Service<GetInt32>::SharedPtr get_auto_white_balance_srv_;
|
||||
rclcpp::Service<SetBool>::SharedPtr set_auto_white_balance_srv_;
|
||||
rclcpp::Service<GetString>::SharedPtr get_sdk_version_srv_;
|
||||
rclcpp::Service<SetString>::SharedPtr switch_ir_camera_srv_;
|
||||
rclcpp::Service<SetString>::SharedPtr set_write_customerdata_srv_;
|
||||
rclcpp::Service<SetString>::SharedPtr set_read_customerdata_srv_;
|
||||
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ir_long_exposure_srv_;
|
||||
std::map<stream_index_pair, rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr>
|
||||
set_auto_exposure_srv_;
|
||||
std::map<stream_index_pair, rclcpp::Service<SetArrays>::SharedPtr> set_ae_roi_srv_;
|
||||
rclcpp::Service<GetDeviceInfo>::SharedPtr get_device_srv_;
|
||||
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_laser_enable_srv_;
|
||||
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ldp_enable_srv_;
|
||||
rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_ldp_status_srv_;
|
||||
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_floor_enable_srv_;
|
||||
rclcpp::Service<SetInt32>::SharedPtr set_fan_work_mode_srv_;
|
||||
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
|
||||
rclcpp::Service<GetInt32>::SharedPtr get_lrm_measure_distance_srv_;
|
||||
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_reset_timestamp_srv_;
|
||||
rclcpp::Service<SetInt32>::SharedPtr set_interleaver_laser_sync_srv_;
|
||||
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_sync_host_time_srv_;
|
||||
rclcpp::Service<SetFilter>::SharedPtr set_filter_srv_;
|
||||
|
||||
bool enable_sync_output_accel_gyro_ = false;
|
||||
std::atomic_bool is_camera_node_initialized_{false};
|
||||
std::mutex device_lock_;
|
||||
std::shared_ptr<std::thread> colorFrameThread_ = nullptr;
|
||||
uint8_t* rgb_buffer_ = nullptr;
|
||||
uint8_t* rgb_point_cloud_buffer_ = nullptr;
|
||||
float* xy_table_data_ = nullptr;
|
||||
float* depth_xy_table_data_ = nullptr;
|
||||
uint8_t* depth_point_cloud_buffer_ = nullptr;
|
||||
bool publish_tf_ = false;
|
||||
bool tf_published_ = false;
|
||||
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_ = nullptr;
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> dynamic_tf_broadcaster_ = nullptr;
|
||||
std::vector<geometry_msgs::msg::TransformStamped> tf_msgs;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr depth_registration_cloud_pub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_;
|
||||
bool enable_point_cloud_ = true;
|
||||
bool enable_colored_point_cloud_ = false;
|
||||
std::recursive_mutex point_cloud_mutex_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_pub_;
|
||||
|
||||
orbbec_camera_msgs::msg::DeviceInfo device_info_;
|
||||
std::string point_cloud_qos_;
|
||||
std::vector<geometry_msgs::msg::TransformStamped> static_tf_msgs_;
|
||||
std::shared_ptr<std::thread> tf_thread_ = nullptr;
|
||||
std::condition_variable tf_cv_;
|
||||
double tf_publish_rate_ = 10.0;
|
||||
std::unique_ptr<camera_info_manager::CameraInfoManager> ir_info_manager_ = nullptr;
|
||||
std::unique_ptr<camera_info_manager::CameraInfoManager> color_info_manager_ = nullptr;
|
||||
std::string color_info_url_;
|
||||
std::string ir_info_url_;
|
||||
std::optional<OBCameraParam> camera_param_;
|
||||
bool enable_d2c_viewer_ = false;
|
||||
std::unique_ptr<D2CViewer> d2c_viewer_ = nullptr;
|
||||
std::map<stream_index_pair, std::atomic_bool> save_images_;
|
||||
std::map<stream_index_pair, int> save_images_count_;
|
||||
int max_save_images_count_ = 10;
|
||||
std::atomic_bool save_point_cloud_{false};
|
||||
std::atomic_bool save_colored_point_cloud_{false};
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr save_images_srv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr save_point_cloud_srv_;
|
||||
std::string depth_filter_config_;
|
||||
bool enable_depth_filter_ = false;
|
||||
bool enable_color_auto_exposure_priority_ = false;
|
||||
bool enable_color_auto_exposure_ = true;
|
||||
bool enable_color_auto_white_balance_ = true;
|
||||
bool enable_depth_auto_exposure_priority_ = false;
|
||||
bool enable_ir_auto_exposure_ = true;
|
||||
bool enable_ir_long_exposure_ = false;
|
||||
bool enable_ldp_ = true;
|
||||
int ldp_power_level_ = -1;
|
||||
int color_rotation_ = -1;
|
||||
// color ae roi
|
||||
int color_ae_roi_left_ = -1;
|
||||
int color_ae_roi_top_ = -1;
|
||||
int color_ae_roi_right_ = -1;
|
||||
int color_ae_roi_bottom_ = -1;
|
||||
int color_exposure_ = -1;
|
||||
int color_gain_ = -1;
|
||||
int color_white_balance_ = -1;
|
||||
int color_ae_max_exposure_ = -1;
|
||||
int color_brightness_ = -1;
|
||||
int color_sharpness_ = -1;
|
||||
int color_gamma_ = -1;
|
||||
int color_saturation_ = -1;
|
||||
int color_constrast_ = -1;
|
||||
int color_hue_ = -1;
|
||||
bool enable_color_backlight_compenstation_ = false;
|
||||
std::string color_powerline_freq_;
|
||||
bool enable_color_decimation_filter_ = false;
|
||||
int color_decimation_filter_scale_ = -1;
|
||||
// depth ae roi
|
||||
int depth_ae_roi_left_ = -1;
|
||||
int depth_ae_roi_top_ = -1;
|
||||
int depth_ae_roi_right_ = -1;
|
||||
int depth_ae_roi_bottom_ = -1;
|
||||
int depth_brightness_ = -1;
|
||||
int ir_exposure_ = -1;
|
||||
int ir_gain_ = -1;
|
||||
int ir_ae_max_exposure_ = -1;
|
||||
int ir_brightness_ = -1;
|
||||
bool enable_right_ir_sequence_id_filter_ = false;
|
||||
int right_ir_sequence_id_filter_id_ = -1;
|
||||
bool enable_left_ir_sequence_id_filter_ = false;
|
||||
int left_ir_sequence_id_filter_id_ = -1;
|
||||
bool enable_frame_sync_ = false;
|
||||
// Only for Gemini2 device
|
||||
std::string disaparity_to_depth_mode_ = "HW";
|
||||
std::string depth_work_mode_;
|
||||
OBMultiDeviceSyncMode sync_mode_ = OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN;
|
||||
std::string sync_mode_str_;
|
||||
int depth_delay_us_ = 0;
|
||||
int color_delay_us_ = 0;
|
||||
int trigger2image_delay_us_ = 0;
|
||||
int trigger_out_delay_us_ = 0;
|
||||
bool trigger_out_enabled_ = false;
|
||||
int frames_per_trigger_ = 2;
|
||||
bool enable_ptp_config_ = false;
|
||||
std::string depth_precision_str_;
|
||||
OB_DEPTH_PRECISION_LEVEL depth_precision_ = OB_PRECISION_0MM8;
|
||||
double depth_precision_float_ = 0.10;
|
||||
// IMU
|
||||
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> imu_publishers_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_gyro_accel_publisher_;
|
||||
bool imu_sync_output_start_ = false;
|
||||
std::map<stream_index_pair, std::string> imu_rate_;
|
||||
std::map<stream_index_pair, std::string> imu_range_;
|
||||
std::map<stream_index_pair, std::string> imu_qos_;
|
||||
std::map<stream_index_pair, bool> imu_started_;
|
||||
double liner_accel_cov_ = 0.0001;
|
||||
double angular_vel_cov_ = 0.0001;
|
||||
bool enable_accel_data_correction_ = true;
|
||||
bool enable_gyro_data_correction_ = true;
|
||||
// mjpeg decoder
|
||||
std::shared_ptr<JPEGDecoder> jpeg_decoder_ = nullptr;
|
||||
uint8_t* rgb_buffer_ = nullptr;
|
||||
bool is_color_frame_decoded_ = false;
|
||||
std::mutex device_lock_;
|
||||
// For color
|
||||
std::queue<std::shared_ptr<ob::FrameSet>> color_frame_queue_;
|
||||
std::shared_ptr<std::thread> colorFrameThread_ = nullptr;
|
||||
std::mutex color_frame_queue_lock_;
|
||||
std::condition_variable color_frame_queue_cv_;
|
||||
|
||||
bool ordered_pc_ = false;
|
||||
bool enable_depth_scale_ = true;
|
||||
std::string device_preset_ = "Default";
|
||||
// filter switch
|
||||
bool enable_decimation_filter_ = false;
|
||||
bool enable_hdr_merge_ = false;
|
||||
bool enable_sequence_id_filter_ = false;
|
||||
bool enable_disaparity_to_depth_ = true;
|
||||
bool enable_threshold_filter_ = false;
|
||||
bool enable_hardware_noise_removal_filter_ = true;
|
||||
bool enable_noise_removal_filter_ = true;
|
||||
bool enable_spatial_filter_ = true;
|
||||
bool enable_temporal_filter_ = false;
|
||||
bool enable_hole_filling_filter_ = false;
|
||||
// filter params
|
||||
int decimation_filter_scale_ = -1;
|
||||
int sequence_id_filter_id_ = -1;
|
||||
int threshold_filter_max_ = -1;
|
||||
int threshold_filter_min_ = -1;
|
||||
float hardware_noise_removal_filter_threshold_ = -1.0;
|
||||
int noise_removal_filter_min_diff_ = 256;
|
||||
int noise_removal_filter_max_size_ = 80;
|
||||
float spatial_filter_alpha_ = -1;
|
||||
int spatial_filter_diff_threshold_ = -1;
|
||||
int spatial_filter_magnitude_ = -1;
|
||||
int spatial_filter_radius_ = -1;
|
||||
float temporal_filter_diff_threshold_ = -1.0;
|
||||
float temporal_filter_weight_ = -1.0;
|
||||
std::string hole_filling_filter_mode_;
|
||||
int hdr_merge_exposure_1_ = -1;
|
||||
int hdr_merge_gain_1_ = -1;
|
||||
int hdr_merge_exposure_2_ = -1;
|
||||
int hdr_merge_gain_2_ = -1;
|
||||
int gmsl_trigger_fd_ = -1;
|
||||
int gmsl_trigger_fps_ = -1;
|
||||
bool enable_gmsl_trigger_ = false;
|
||||
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr filter_status_pub_;
|
||||
nlohmann::json filter_status_;
|
||||
std::string align_mode_ = "HW";
|
||||
std::unique_ptr<diagnostic_updater::Updater> diagnostic_updater_ = nullptr;
|
||||
double diagnostic_period_ = 1.0;
|
||||
bool enable_laser_ = false;
|
||||
std::unique_ptr<ob::Align> align_filter_ = nullptr;
|
||||
OBStreamType align_target_stream_ = OB_STREAM_COLOR;
|
||||
bool retry_on_usb3_detection_failure_ = false;
|
||||
std::atomic_bool is_camera_node_initialized_{false};
|
||||
int laser_energy_level_ = -1;
|
||||
ob::PointCloudFilter depth_point_cloud_filter_;
|
||||
ob::PointCloudFilter color_point_cloud_filter_;
|
||||
std::optional<OBCalibrationParam> calibration_param_;
|
||||
std::optional<OBXYTables> xy_tables_;
|
||||
float* xy_table_data_ = nullptr;
|
||||
uint32_t xy_table_data_size_ = 0;
|
||||
uint8_t* rgb_point_cloud_buffer_ = nullptr;
|
||||
uint32_t rgb_point_cloud_buffer_size_ = 0;
|
||||
std::optional<OBXYTables> depth_xy_tables_;
|
||||
float* depth_xy_table_data_ = nullptr;
|
||||
uint32_t depth_xy_table_data_size_ = 0;
|
||||
uint8_t* depth_point_cloud_buffer_ = nullptr;
|
||||
uint32_t depth_point_cloud_buffer_size_ = 0;
|
||||
int min_depth_limit_ = 0;
|
||||
int max_depth_limit_ = 0;
|
||||
std::string time_domain_ = "global"; // device, system, global
|
||||
std::string exposure_range_mode_ = "default";
|
||||
std::string load_config_json_file_path_ = "";
|
||||
std::string export_config_json_file_path_ = "";
|
||||
// soft ware trigger
|
||||
rclcpp::TimerBase::SharedPtr software_trigger_timer_;
|
||||
rclcpp::TimerBase::SharedPtr diagnostic_timer_;
|
||||
std::chrono::milliseconds software_trigger_period_{33};
|
||||
std::string time_domain_ = "device"; // device, system, global
|
||||
bool enable_heartbeat_ = false;
|
||||
bool enable_color_undistortion_ = false;
|
||||
std::shared_ptr<image_publisher> color_undistortion_publisher_;
|
||||
bool has_first_color_frame_ = false;
|
||||
bool use_intra_process_ = false;
|
||||
std::string cloud_frame_id_;
|
||||
std::vector<std::shared_ptr<ob::Filter>> depth_filter_list_;
|
||||
std::vector<std::shared_ptr<ob::Filter>> color_filter_list_;
|
||||
std::vector<std::shared_ptr<ob::Filter>> left_ir_filter_list_;
|
||||
std::vector<std::shared_ptr<ob::Filter>> right_ir_filter_list_;
|
||||
|
||||
// interleave AE
|
||||
std::string interleave_ae_mode_ = "hdr"; // hdr or laser
|
||||
bool interleave_frame_enable_ = false;
|
||||
bool interleave_skip_enable_ = false;
|
||||
int interleave_skip_index_ = 1;
|
||||
|
||||
// hdr and laser interleave params
|
||||
int hdr_index1_laser_control_ = 1;
|
||||
int hdr_index1_depth_exposure_ = 1;
|
||||
int hdr_index1_depth_gain_ = 16;
|
||||
int hdr_index1_ir_brightness_ = 20;
|
||||
int hdr_index1_ir_ae_max_exposure_ = 2000;
|
||||
int hdr_index0_laser_control_ = 1;
|
||||
int hdr_index0_depth_exposure_ = 7500;
|
||||
int hdr_index0_depth_gain_ = 16;
|
||||
int hdr_index0_ir_brightness_ = 60;
|
||||
int hdr_index0_ir_ae_max_exposure_ = 10000;
|
||||
|
||||
int laser_index1_laser_control_ = 0;
|
||||
int laser_index1_depth_exposure_ = 3000;
|
||||
int laser_index1_depth_gain_ = 16;
|
||||
int laser_index1_ir_brightness_ = 60;
|
||||
int laser_index1_ir_ae_max_exposure_ = 7000;
|
||||
int laser_index0_laser_control_ = 1;
|
||||
int laser_index0_depth_exposure_ = 3000;
|
||||
int laser_index0_depth_gain_ = 16;
|
||||
int laser_index0_ir_brightness_ = 60;
|
||||
int laser_index0_ir_ae_max_exposure_ = 17000;
|
||||
|
||||
int disparity_range_mode_ = -1;
|
||||
int disparity_search_offset_ = -1;
|
||||
bool disparity_offset_config_ = false;
|
||||
int offset_index0_ = -1;
|
||||
int offset_index1_ = -1;
|
||||
|
||||
std::string frame_aggregate_mode_ = "ANY"; // # full_frame, color_frame, ANY or disable
|
||||
|
||||
// lidar
|
||||
std::string lidar_format_ = "ANY";
|
||||
@@ -504,7 +243,7 @@ class OBLidarNode {
|
||||
std::string echo_mode_ = "single channel";
|
||||
std::map<stream_index_pair, int> rate_int_;
|
||||
std::map<stream_index_pair, OBLiDARScanRate> rate_;
|
||||
std::string frame_id_ = "scan";
|
||||
std::map<stream_index_pair, std::string> frame_id_;
|
||||
float min_angle_ = -135.0;
|
||||
float max_angle_ = 135.0;
|
||||
float min_range_ = 0.05;
|
||||
|
||||
@@ -60,8 +60,7 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('frame_id', default_value='scan'),
|
||||
DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
|
||||
DeclareLaunchArgument('lidar_format', default_value='ANY'),
|
||||
DeclareLaunchArgument('lidar_rate', default_value='0'),
|
||||
DeclareLaunchArgument('scan_rate', default_value='0'),
|
||||
DeclareLaunchArgument('lidar_rate', default_value='30'),
|
||||
DeclareLaunchArgument('min_angle', default_value='-135.0'),
|
||||
DeclareLaunchArgument('max_angle', default_value='135.0'),
|
||||
DeclareLaunchArgument('min_range', default_value='0.05'),
|
||||
|
||||
@@ -144,17 +144,22 @@ void OBLidarNode::getParameters() {
|
||||
param_name = stream_name_[stream_index] + "_rate";
|
||||
setAndGetNodeParameter(rate_int_[stream_index], param_name, 0);
|
||||
rate_[stream_index] = OBScanRateFromInt(rate_int_[stream_index]);
|
||||
RCLCPP_INFO_STREAM(logger_, "rate_ " << magic_enum::enum_name(rate_[LIDAR]) << "format_"
|
||||
RCLCPP_INFO_STREAM(logger_, "rate_ " << magic_enum::enum_name(rate_[LIDAR]) << " format_"
|
||||
<< magic_enum::enum_name(format_[LIDAR]));
|
||||
param_name = stream_name_[stream_index] + "_frame_id";
|
||||
std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame";
|
||||
setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id);
|
||||
std::string default_optical_frame_id =
|
||||
camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame";
|
||||
param_name = stream_name_[stream_index] + "_optical_frame_id";
|
||||
setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
|
||||
}
|
||||
|
||||
setAndGetNodeParameter<bool>(publish_tf_, "publish_tf", true);
|
||||
setAndGetNodeParameter<double>(tf_publish_rate_, "tf_publish_rate", 0.0);
|
||||
setAndGetNodeParameter<std::string>(time_domain_, "time_domain", "global");
|
||||
setAndGetNodeParameter<bool>(enable_heartbeat_, "enable_heartbeat", false);
|
||||
setAndGetNodeParameter<std::string>(echo_mode_, "echo_mode", "single channel");
|
||||
setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default");
|
||||
setAndGetNodeParameter<std::string>(frame_id_, "frame_id", "scan");
|
||||
setAndGetNodeParameter<float>(min_angle_, "min_angle", -135.0);
|
||||
setAndGetNodeParameter<float>(max_angle_, "max_angle", 135.0);
|
||||
setAndGetNodeParameter<float>(min_range_, "min_range", 0.05);
|
||||
@@ -300,7 +305,7 @@ void OBLidarNode::setupPublishers() {
|
||||
point_cloud_qos_profile));
|
||||
} else if (format_[LIDAR] == OB_FORMAT_LIDAR_POINT ||
|
||||
format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_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),
|
||||
point_cloud_qos_profile));
|
||||
}
|
||||
@@ -382,11 +387,16 @@ void OBLidarNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set)
|
||||
}
|
||||
try {
|
||||
RCLCPP_INFO_ONCE(logger_, "New frame received");
|
||||
if (!tf_published_) {
|
||||
publishStaticTransforms();
|
||||
tf_published_ = true;
|
||||
}
|
||||
if (format_[LIDAR] == OB_FORMAT_LIDAR_SCAN) {
|
||||
publishScan(frame_set);
|
||||
} else if (format_[LIDAR] == OB_FORMAT_LIDAR_POINT ||
|
||||
format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT) {
|
||||
} else if (format_[LIDAR] == OB_FORMAT_LIDAR_POINT) {
|
||||
publishPointCloud(frame_set);
|
||||
} else if (format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT) {
|
||||
publishSpherePointCloud(frame_set);
|
||||
}
|
||||
} catch (const ob::Error &e) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.getMessage());
|
||||
@@ -406,19 +416,11 @@ void OBLidarNode::publishScan(std::shared_ptr<ob::FrameSet> frame_set) {
|
||||
auto lidar_frame = frame_set->getFrame(OB_FRAME_LIDAR_POINTS);
|
||||
auto *scans_data = reinterpret_cast<OBLiDARScanPoint *>(lidar_frame->getData());
|
||||
auto scan_count = lidar_frame->getDataSize() / sizeof(OBLiDARScanPoint);
|
||||
// bool valid_point = points[i].z >= min_depth && points[i].z <= max_depth;
|
||||
// if (valid_point || ordered_pc_) {
|
||||
// *iter_x = static_cast<float>(points[i].x / 1000.0);
|
||||
// *iter_y = static_cast<float>(points[i].y / 1000.0);
|
||||
// *iter_z = static_cast<float>(points[i].z / 1000.0);
|
||||
// ++iter_x, ++iter_y, ++iter_z;
|
||||
// valid_count++;
|
||||
// }
|
||||
auto frame_timestamp = getFrameTimestampUs(lidar_frame);
|
||||
auto timestamp = fromUsToROSTime(frame_timestamp);
|
||||
auto scan_msg = std::make_unique<sensor_msgs::msg::LaserScan>();
|
||||
scan_msg->header.stamp = timestamp;
|
||||
scan_msg->header.frame_id = frame_id_;
|
||||
scan_msg->header.frame_id = frame_id_[LIDAR];
|
||||
scan_msg->angle_min = 0.7853981852531433;
|
||||
scan_msg->angle_max = 5.495169162750244;
|
||||
scan_msg->angle_increment = 0.0026179938577115536;
|
||||
@@ -429,9 +431,6 @@ void OBLidarNode::publishScan(std::shared_ptr<ob::FrameSet> frame_set) {
|
||||
scan_msg->ranges.resize(scan_count);
|
||||
scan_msg->intensities.resize(scan_count);
|
||||
for (size_t i = 0; i < scan_count; i++) {
|
||||
// RCLCPP_INFO_STREAM(logger_, " angle: " << scans_data[i].angle
|
||||
// << " distance: " << scans_data[i].distance
|
||||
// << " intensity: " << scans_data[i].intensity);
|
||||
if (scans_data->distance < min_range_ && scans_data->distance > max_range_) {
|
||||
scans_data++;
|
||||
continue;
|
||||
@@ -441,19 +440,53 @@ void OBLidarNode::publishScan(std::shared_ptr<ob::FrameSet> frame_set) {
|
||||
}
|
||||
filterScan(*scan_msg);
|
||||
scan_pub_->publish(std::move(scan_msg));
|
||||
// RCLCPP_INFO_STREAM(logger_, "getFormat "<<magic_enum::enum_name(lidar_frame->getFormat()));
|
||||
// RCLCPP_INFO_STREAM(logger_, "getDataSize "<<lidar_frame->getDataSize());
|
||||
// RCLCPP_INFO_STREAM(logger_, "getType "<<lidar_frame->getType());
|
||||
// RCLCPP_INFO_STREAM(logger_, "getSystemTimeStampUs "<<lidar_frame->getSystemTimeStampUs());
|
||||
// RCLCPP_INFO_STREAM(logger_, "getGlobalTimeStampUs "<<lidar_frame->getGlobalTimeStampUs());
|
||||
// RCLCPP_INFO_STREAM(logger_, "getTimeStampUs "<<lidar_frame->getTimeStampUs());
|
||||
// RCLCPP_INFO_STREAM(logger_, "getIndex "<<lidar_frame->getIndex());
|
||||
// RCLCPP_INFO_STREAM(logger_, "getMetadataSize "<<lidar_frame->getMetadataSize());
|
||||
}
|
||||
|
||||
void OBLidarNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
|
||||
(void)frame_set;
|
||||
// RCLCPP_INFO_STREAM(logger_, "publishPointCloud ");
|
||||
if (frame_set == nullptr) {
|
||||
return;
|
||||
}
|
||||
auto lidar_frame = frame_set->getFrame(OB_FRAME_LIDAR_POINTS);
|
||||
auto *point_data = reinterpret_cast<OBLiDARPoint *>(lidar_frame->getData());
|
||||
auto point_count = lidar_frame->getDataSize() / sizeof(OBLiDARScanPoint);
|
||||
auto frame_timestamp = getFrameTimestampUs(lidar_frame);
|
||||
auto timestamp = fromUsToROSTime(frame_timestamp);
|
||||
auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
|
||||
sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
|
||||
modifier.setPointCloud2Fields(5, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
|
||||
sensor_msgs::msg::PointField::FLOAT32, "z", 1,
|
||||
sensor_msgs::msg::PointField::FLOAT32, "reflectivity", 1,
|
||||
sensor_msgs::msg::PointField::UINT8, "tag", 1,
|
||||
sensor_msgs::msg::PointField::UINT8);
|
||||
modifier.resize(point_count);
|
||||
point_cloud_msg->header.stamp = timestamp;
|
||||
point_cloud_msg->header.frame_id = frame_id_[LIDAR];
|
||||
point_cloud_msg->height = 1;
|
||||
point_cloud_msg->width = point_count;
|
||||
point_cloud_msg->is_dense = true;
|
||||
point_cloud_msg->is_bigendian = false;
|
||||
point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step;
|
||||
point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step);
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud_msg, "x");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
|
||||
sensor_msgs::PointCloud2Iterator<uint8_t> iter_reflectivity(*point_cloud_msg, "reflectivity");
|
||||
sensor_msgs::PointCloud2Iterator<uint8_t> iter_tag(*point_cloud_msg, "tag");
|
||||
for (size_t i = 0; i < point_count;
|
||||
++i, ++iter_x, ++iter_y, ++iter_z, ++iter_reflectivity, ++iter_tag) {
|
||||
*iter_x = static_cast<float>(point_data[i].x / 1000.0);
|
||||
*iter_y = static_cast<float>(point_data[i].y / 1000.0);
|
||||
*iter_z = static_cast<float>(point_data[i].z / 1000.0);
|
||||
*iter_reflectivity = point_data[i].reflectivity;
|
||||
*iter_tag = point_data[i].tag;
|
||||
}
|
||||
*point_cloud_msg = filterPointCloud(*point_cloud_msg);
|
||||
point_cloud_pub_->publish(std::move(point_cloud_msg));
|
||||
}
|
||||
|
||||
void OBLidarNode::publishSpherePointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
|
||||
(void)frame_set;
|
||||
}
|
||||
|
||||
uint64_t OBLidarNode::getFrameTimestampUs(const std::shared_ptr<ob::Frame> &frame) {
|
||||
@@ -498,5 +531,168 @@ void OBLidarNode::filterScan(sensor_msgs::msg::LaserScan &scan) {
|
||||
}
|
||||
}
|
||||
}
|
||||
sensor_msgs::msg::PointCloud2 OBLidarNode::filterPointCloud(
|
||||
sensor_msgs::msg::PointCloud2 &point_cloud) const {
|
||||
// Initialize the filtered point cloud
|
||||
sensor_msgs::msg::PointCloud2 filtered_point_cloud;
|
||||
filtered_point_cloud.header = point_cloud.header;
|
||||
filtered_point_cloud.height = point_cloud.height;
|
||||
filtered_point_cloud.width = point_cloud.width;
|
||||
filtered_point_cloud.is_dense = point_cloud.is_dense;
|
||||
filtered_point_cloud.is_bigendian = point_cloud.is_bigendian;
|
||||
filtered_point_cloud.fields = point_cloud.fields;
|
||||
filtered_point_cloud.point_step = point_cloud.point_step;
|
||||
|
||||
// Convert filter angles from degrees to radians and normalize to [0, 2π]
|
||||
double max_angle = deg2rad(max_angle_);
|
||||
double min_angle = deg2rad(min_angle_);
|
||||
max_angle = std::fmod(max_angle + M_PI, 2 * M_PI);
|
||||
min_angle = std::fmod(min_angle + M_PI, 2 * M_PI);
|
||||
if (min_angle < 0) {
|
||||
min_angle += 2 * M_PI;
|
||||
}
|
||||
if (max_angle < 0) {
|
||||
max_angle += 2 * M_PI;
|
||||
}
|
||||
|
||||
// Swap angles if min is greater than max
|
||||
if (min_angle > max_angle) {
|
||||
std::swap(min_angle, max_angle);
|
||||
}
|
||||
|
||||
// Reserve space for filtered point cloud data
|
||||
filtered_point_cloud.data.reserve(point_cloud.data.size());
|
||||
|
||||
// Create iterators for each field
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud, "x");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud, "y");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud, "z");
|
||||
sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(point_cloud, "intensity");
|
||||
|
||||
// Process each point
|
||||
for (size_t i = 0; i < point_cloud.height * point_cloud.width;
|
||||
++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity) {
|
||||
float x = *iter_x;
|
||||
float y = *iter_y;
|
||||
float z = *iter_z;
|
||||
|
||||
// Calculate distance from origin
|
||||
float distance = std::sqrt(x * x + y * y + z * z);
|
||||
|
||||
// Calculate angle and normalize to [0, 2π]
|
||||
float angle = std::atan2(y, x);
|
||||
angle = std::fmod(angle + 2 * M_PI, 2 * M_PI);
|
||||
|
||||
// Check if point is within both angle and range limits
|
||||
bool is_angle_in_range = (angle >= min_angle && angle <= max_angle);
|
||||
bool is_range_in_range = (distance >= min_range_ && distance <= max_range_);
|
||||
|
||||
if (is_angle_in_range && is_range_in_range) {
|
||||
// Keep points within the specified range
|
||||
filtered_point_cloud.data.insert(filtered_point_cloud.data.end(),
|
||||
point_cloud.data.begin() + i * point_cloud.point_step,
|
||||
point_cloud.data.begin() + (i + 1) * point_cloud.point_step);
|
||||
} else {
|
||||
// Fill zero values for filtered out points
|
||||
filtered_point_cloud.data.insert(filtered_point_cloud.data.end(), point_cloud.point_step, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// Update row step and resize data
|
||||
filtered_point_cloud.row_step = filtered_point_cloud.width * filtered_point_cloud.point_step;
|
||||
filtered_point_cloud.data.resize(filtered_point_cloud.height * filtered_point_cloud.row_step);
|
||||
|
||||
return filtered_point_cloud;
|
||||
}
|
||||
|
||||
|
||||
void OBLidarNode::publishStaticTransforms() {
|
||||
if (!publish_tf_) {
|
||||
return;
|
||||
}
|
||||
static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
|
||||
dynamic_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
|
||||
calcAndPublishStaticTransform();
|
||||
if (tf_publish_rate_ > 0) {
|
||||
tf_thread_ = std::make_shared<std::thread>([this]() { publishDynamicTransforms(); });
|
||||
} else {
|
||||
static_tf_broadcaster_->sendTransform(static_tf_msgs_);
|
||||
}
|
||||
}
|
||||
|
||||
void OBLidarNode::calcAndPublishStaticTransform() {
|
||||
tf2::Quaternion quaternion_optical, zero_rot;
|
||||
zero_rot.setRPY(0.0, 0.0, 0.0);
|
||||
quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
|
||||
tf2::Vector3 zero_trans(0, 0, 0);
|
||||
auto base_stream_profile = stream_profile_[base_stream_];
|
||||
if (!base_stream_profile) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to get base stream profile");
|
||||
return;
|
||||
}
|
||||
CHECK_NOTNULL(base_stream_profile.get());
|
||||
for (const auto &item : stream_profile_) {
|
||||
auto stream_index = item.first;
|
||||
|
||||
auto stream_profile = item.second;
|
||||
if (!stream_profile) {
|
||||
continue;
|
||||
}
|
||||
OBExtrinsic ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
|
||||
|
||||
auto Q = rotationMatrixToQuaternion(ex.rot);
|
||||
Q = quaternion_optical * Q * quaternion_optical.inverse();
|
||||
tf2::Vector3 trans(ex.trans[0], ex.trans[1], ex.trans[2]);
|
||||
auto timestamp = node_->now();
|
||||
if (stream_index.first != base_stream_.first) {
|
||||
if (stream_index.first == OB_STREAM_IR_RIGHT && base_stream_.first == OB_STREAM_DEPTH) {
|
||||
trans[0] = std::abs(trans[0]); // because left and right ir calibration is error
|
||||
}
|
||||
publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], frame_id_[stream_index]);
|
||||
}
|
||||
publishStaticTF(timestamp, zero_trans, quaternion_optical, frame_id_[stream_index],
|
||||
optical_frame_id_[stream_index]);
|
||||
RCLCPP_INFO_STREAM(logger_, "Publishing static transform from " << stream_name_[stream_index]
|
||||
<< " to "
|
||||
<< stream_name_[base_stream_]);
|
||||
RCLCPP_INFO_STREAM(logger_, "Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]);
|
||||
RCLCPP_INFO_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
|
||||
<< ", " << Q.getW());
|
||||
}
|
||||
}
|
||||
void OBLidarNode::publishStaticTF(const rclcpp::Time &t, const tf2::Vector3 &trans,
|
||||
const tf2::Quaternion &q, const std::string &from,
|
||||
const std::string &to) {
|
||||
geometry_msgs::msg::TransformStamped msg;
|
||||
msg.header.stamp = t;
|
||||
msg.header.frame_id = from;
|
||||
msg.child_frame_id = to;
|
||||
msg.transform.translation.x = trans[2] / 1000.0;
|
||||
msg.transform.translation.y = -trans[0] / 1000.0;
|
||||
msg.transform.translation.z = -trans[1] / 1000.0;
|
||||
msg.transform.rotation.x = q.getX();
|
||||
msg.transform.rotation.y = q.getY();
|
||||
msg.transform.rotation.z = q.getZ();
|
||||
msg.transform.rotation.w = q.getW();
|
||||
static_tf_msgs_.push_back(msg);
|
||||
}
|
||||
|
||||
void OBLidarNode::publishDynamicTransforms() {
|
||||
RCLCPP_WARN(logger_, "Publishing dynamic camera transforms (/tf) at %g Hz", tf_publish_rate_);
|
||||
std::mutex mu;
|
||||
std::unique_lock<std::mutex> lock(mu);
|
||||
while (rclcpp::ok() && is_running_) {
|
||||
tf_cv_.wait_for(lock, std::chrono::milliseconds((int)(1000.0 / tf_publish_rate_)),
|
||||
[this] { return (!(is_running_)); });
|
||||
{
|
||||
rclcpp::Time t = node_->now();
|
||||
for (auto &msg : static_tf_msgs_) {
|
||||
msg.header.stamp = t;
|
||||
}
|
||||
dynamic_tf_broadcaster_->sendTransform(static_tf_msgs_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace orbbec_lidar
|
||||
} // namespace orbbec_camera
|
||||
|
||||
@@ -971,4 +971,5 @@ double rad2deg(double rad) {
|
||||
}
|
||||
return angle_degrees;
|
||||
}
|
||||
|
||||
} // namespace orbbec_camera
|
||||
|
||||
Reference in New Issue
Block a user