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

Update scan push

Browse Source
This commit is contained in:
jj
2025-06-09 21:24:06 +08:00
parent b1ee682007
commit f06fc6de7f
8 changed files with 445 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h
+1 -1
View File
@@ -84,7 +84,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
std::unique_ptr<ob::Context> ctx_ = nullptr; std::unique_ptr<ob::Context> ctx_ = nullptr;
rclcpp::Logger logger_; rclcpp::Logger logger_;
std::unique_ptr<OBCameraNode> ob_camera_node_ = nullptr; std::unique_ptr<OBCameraNode> ob_camera_node_ = nullptr;
std::unique_ptr<OBLidarNode> ob_lidar_node_ = nullptr; std::unique_ptr<orbbec_lidar::OBLidarNode> ob_lidar_node_ = nullptr;
std::shared_ptr<ob::Device> device_ = nullptr; std::shared_ptr<ob::Device> device_ = nullptr;
std::shared_ptr<ob::DeviceInfo> device_info_ = nullptr; std::shared_ptr<ob::DeviceInfo> device_info_ = nullptr;
std::atomic_bool is_alive_{false}; std::atomic_bool is_alive_{false};
orbbec_camera/include/orbbec_camera/ob_lidar_node.h
+50 -7
View File
@@ -26,8 +26,9 @@
#include <utility> #include <utility>
#include <vector> #include <vector>
#include <atomic> #include <atomic>
#include "ob_camera_node.h" // #include "ob_lidar_node.h"
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp> #include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp> #include <sensor_msgs/point_cloud2_iterator.hpp>
#include <tf2_ros/static_transform_broadcaster.h> #include <tf2_ros/static_transform_broadcaster.h>
@@ -102,6 +103,7 @@
#define DEVICE_PATH "/dev/camsync" #define DEVICE_PATH "/dev/camsync"
namespace orbbec_camera { namespace orbbec_camera {
namespace orbbec_lidar {
using GetDeviceInfo = orbbec_camera_msgs::srv::GetDeviceInfo; using GetDeviceInfo = orbbec_camera_msgs::srv::GetDeviceInfo;
using Extrinsics = orbbec_camera_msgs::msg::Extrinsics; using Extrinsics = orbbec_camera_msgs::msg::Extrinsics;
using SetInt32 = orbbec_camera_msgs::srv::SetInt32; using SetInt32 = orbbec_camera_msgs::srv::SetInt32;
@@ -113,6 +115,17 @@ using GetBool = orbbec_camera_msgs::srv::GetBool;
using SetFilter = orbbec_camera_msgs::srv::SetFilter; using SetFilter = orbbec_camera_msgs::srv::SetFilter;
using SetArrays = orbbec_camera_msgs::srv::SetArrays; using SetArrays = orbbec_camera_msgs::srv::SetArrays;
typedef std::pair<ob_stream_type, int> stream_index_pair;
const stream_index_pair LIDAR{OB_STREAM_LIDAR, 0};
const stream_index_pair GYRO{OB_STREAM_GYRO, 0};
const stream_index_pair ACCEL{OB_STREAM_ACCEL, 0};
const std::vector<stream_index_pair> IMAGE_STREAMS = {LIDAR};
const std::vector<stream_index_pair> HID_STREAMS = {GYRO, ACCEL};
class OBLidarNode { class OBLidarNode {
public: public:
OBLidarNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device, OBLidarNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device,
@@ -130,20 +143,37 @@ class OBLidarNode {
void rebootDevice(); void rebootDevice();
private: void startStreams();
private:
void setupDevices(); void setupDevices();
// void selectBaseStream(); void selectBaseStream();
// void setupProfiles(); void setupProfiles();
void getParameters(); void getParameters();
void setupTopics(); void setupTopics();
void setupPipelineConfig();
void printSensorProfiles(const std::shared_ptr<ob::Sensor>& sensor);
void setupPublishers(); void setupPublishers();
void stopStreams();
void onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set);
void publishScan(std::shared_ptr<ob::FrameSet> frame_set);
void publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
uint64_t getFrameTimestampUs(const std::shared_ptr<ob::Frame>& frame);
void filterScan(sensor_msgs::msg::LaserScan& scan);
private: private:
rclcpp::Node* node_ = nullptr; rclcpp::Node* node_ = nullptr;
std::shared_ptr<ob::Device> device_ = nullptr; std::shared_ptr<ob::Device> device_ = nullptr;
@@ -171,7 +201,6 @@ class OBLidarNode {
std::map<stream_index_pair, int> width_; std::map<stream_index_pair, int> width_;
std::map<stream_index_pair, int> height_; std::map<stream_index_pair, int> height_;
std::map<stream_index_pair, int> fps_; std::map<stream_index_pair, int> fps_;
std::map<stream_index_pair, std::string> frame_id_;
std::map<stream_index_pair, std::string> optical_frame_id_; std::map<stream_index_pair, std::string> optical_frame_id_;
std::map<stream_index_pair, std::string> depth_aligned_frame_id_; std::map<stream_index_pair, std::string> depth_aligned_frame_id_;
std::string camera_link_frame_id_; std::string camera_link_frame_id_;
@@ -181,10 +210,10 @@ class OBLidarNode {
std::map<stream_index_pair, ob_format> format_; std::map<stream_index_pair, ob_format> format_;
std::map<stream_index_pair, std::string> format_str_; std::map<stream_index_pair, std::string> format_str_;
std::map<stream_index_pair, int> image_format_; std::map<stream_index_pair, int> image_format_;
std::map<stream_index_pair, std::vector<std::shared_ptr<ob::VideoStreamProfile>>> std::map<stream_index_pair, std::vector<std::shared_ptr<ob::LiDARStreamProfile>>>
supported_profiles_; supported_profiles_;
std::map<stream_index_pair, std::shared_ptr<ob::StreamProfile>> stream_profile_; std::map<stream_index_pair, std::shared_ptr<ob::StreamProfile>> stream_profile_;
stream_index_pair base_stream_ = DEPTH; stream_index_pair base_stream_ = LIDAR;
std::map<stream_index_pair, uint32_t> seq_; std::map<stream_index_pair, uint32_t> seq_;
std::map<stream_index_pair, cv::Mat> images_; std::map<stream_index_pair, cv::Mat> images_;
std::map<stream_index_pair, std::string> encoding_; std::map<stream_index_pair, std::string> encoding_;
@@ -241,6 +270,7 @@ class OBLidarNode {
std::shared_ptr<tf2_ros::TransformBroadcaster> dynamic_tf_broadcaster_ = nullptr; std::shared_ptr<tf2_ros::TransformBroadcaster> dynamic_tf_broadcaster_ = nullptr;
std::vector<geometry_msgs::msg::TransformStamped> tf_msgs; std::vector<geometry_msgs::msg::TransformStamped> tf_msgs;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr depth_registration_cloud_pub_; 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_; rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_;
bool enable_point_cloud_ = true; bool enable_point_cloud_ = true;
bool enable_colored_point_cloud_ = false; bool enable_colored_point_cloud_ = false;
@@ -467,6 +497,19 @@ class OBLidarNode {
int offset_index1_ = -1; int offset_index1_ = -1;
std::string frame_aggregate_mode_ = "ANY"; // # full_frame, color_frame, ANY or disable std::string frame_aggregate_mode_ = "ANY"; // # full_frame, color_frame, ANY or disable
// lidar
std::string lidar_format_ = "ANY";
int lidar_rate_ = 0;
std::string echo_mode_ = "single channel"; 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";
float min_angle_ = -135.0;
float max_angle_ = 135.0;
float min_range_ = 0.05;
float max_range_ = 30.0;
}; };
} // namespace orbbec_lidar
} // namespace orbbec_camera } // namespace orbbec_camera
orbbec_camera/include/orbbec_camera/utils.h
+6
View File
@@ -141,6 +141,8 @@ std::string getObSDKVersion();
OBFormat OBFormatFromString(const std::string& format); OBFormat OBFormatFromString(const std::string& format);
OBLiDARScanRate OBScanRateFromInt(const int rate);
std::string OBFormatToString(const OBFormat& format); std::string OBFormatToString(const OBFormat& format);
std::ostream& operator<<(std::ostream& os, const OBFormat& rhs); std::ostream& operator<<(std::ostream& os, const OBFormat& rhs);
@@ -194,4 +196,8 @@ cv::Mat undistortImage(const cv::Mat& image, const OBCameraIntrinsic& intrinsic,
const OBCameraDistortion& distortion); const OBCameraDistortion& distortion);
std::string getDistortionModels(OBCameraDistortion distortion); std::string getDistortionModels(OBCameraDistortion distortion);
double deg2rad(double deg);
double rad2deg(double rad);
} // namespace orbbec_camera } // namespace orbbec_camera
orbbec_camera/launch/lidar.launch.py
+7 -1
View File
@@ -57,9 +57,15 @@ def generate_launch_description():
DeclareLaunchArgument('upgrade_firmware', default_value=''), DeclareLaunchArgument('upgrade_firmware', default_value=''),
DeclareLaunchArgument('connection_delay', default_value='10'), DeclareLaunchArgument('connection_delay', default_value='10'),
DeclareLaunchArgument('publish_tf', default_value='true'), DeclareLaunchArgument('publish_tf', default_value='true'),
DeclareLaunchArgument('frame_id', default_value='scan'),
DeclareLaunchArgument('tf_publish_rate', default_value='0.0'), DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
DeclareLaunchArgument('lidar_format', default_value='ANY'), DeclareLaunchArgument('lidar_format', default_value='ANY'),
DeclareLaunchArgument('lidar_rate', default_value='0'),
DeclareLaunchArgument('scan_rate', default_value='0'), DeclareLaunchArgument('scan_rate', default_value='0'),
DeclareLaunchArgument('min_angle', default_value='-135.0'),
DeclareLaunchArgument('max_angle', default_value='135.0'),
DeclareLaunchArgument('min_range', default_value='0.05'),
DeclareLaunchArgument('max_range', default_value='30.0'),
DeclareLaunchArgument('echo_mode', default_value='single channel'), DeclareLaunchArgument('echo_mode', default_value='single channel'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'), DeclareLaunchArgument('point_cloud_qos', default_value='default'),
# Network device settings: default enumerate_net_device is set to true, which will automatically enumerate network devices # Network device settings: default enumerate_net_device is set to true, which will automatically enumerate network devices
@@ -69,7 +75,7 @@ def generate_launch_description():
DeclareLaunchArgument('net_device_ip', default_value=''), DeclareLaunchArgument('net_device_ip', default_value=''),
DeclareLaunchArgument('net_device_port', default_value='0'), DeclareLaunchArgument('net_device_port', default_value='0'),
DeclareLaunchArgument('log_level', default_value='none'), DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system DeclareLaunchArgument('time_domain', default_value='device'),# global, device, system
DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('config_file_path', default_value=''),
DeclareLaunchArgument('enable_heartbeat', default_value='false'), DeclareLaunchArgument('enable_heartbeat', default_value='false'),
] ]
orbbec_camera/src/ob_camera_node_driver.cpp
+12 -8
View File
@@ -279,6 +279,7 @@ void OBCameraNodeDriver::resetDevice() {
std::lock_guard<decltype(device_lock_)> device_lock(device_lock_); std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
{ {
ob_camera_node_.reset(); ob_camera_node_.reset();
ob_lidar_node_.reset();
device_.reset(); device_.reset();
device_info_.reset(); device_info_.reset();
device_connected_ = false; device_connected_ = false;
@@ -450,8 +451,8 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_, ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_,
node_options_.use_intra_process_comms()); node_options_.use_intra_process_comms());
} else if (device_type_ == "lidar") { } else if (device_type_ == "lidar") {
ob_lidar_node_ = std::make_unique<OBLidarNode>(this, device_, parameters_, ob_lidar_node_ = std::make_unique<orbbec_lidar::OBLidarNode>(
node_options_.use_intra_process_comms()); this, device_, parameters_, node_options_.use_intra_process_comms());
} }
initialized = true; initialized = true;
@@ -512,12 +513,15 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
std::placeholders::_2, std::placeholders::_3), std::placeholders::_2, std::placeholders::_3),
false); false);
} }
// if (ob_camera_node_) { if (ob_camera_node_) {
// ob_camera_node_->startIMU(); ob_camera_node_->startIMU();
// ob_camera_node_->startStreams(); ob_camera_node_->startStreams();
// } else { } else if (ob_lidar_node_) {
// RCLCPP_INFO_STREAM(logger_, "ob_camera_node_ is nullptr"); // ob_lidar_node_->startIMU();
// } ob_lidar_node_->startStreams();
} else {
RCLCPP_INFO_STREAM(logger_, "ob_camera_node_ is nullptr");
}
} // namespace orbbec_camera } // namespace orbbec_camera
orbbec_camera/src/ob_lidar_node.cpp
+326 -158
View File
@@ -32,6 +32,7 @@
#endif #endif
namespace orbbec_camera { namespace orbbec_camera {
namespace orbbec_lidar {
using namespace std::chrono_literals; using namespace std::chrono_literals;
OBLidarNode::OBLidarNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device, OBLidarNode::OBLidarNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
@@ -73,7 +74,7 @@ OBLidarNode::~OBLidarNode() noexcept { clean(); }
void OBLidarNode::rebootDevice() { void OBLidarNode::rebootDevice() {
RCLCPP_WARN_STREAM(logger_, "Reboot device"); RCLCPP_WARN_STREAM(logger_, "Reboot device");
// clean(); clean();
if (device_) { if (device_) {
device_->reboot(); device_->reboot();
RCLCPP_WARN_STREAM(logger_, "Reboot device DONE"); RCLCPP_WARN_STREAM(logger_, "Reboot device DONE");
@@ -95,7 +96,7 @@ void OBLidarNode::clean() noexcept {
} }
RCLCPP_WARN_STREAM(logger_, "stop streams"); RCLCPP_WARN_STREAM(logger_, "stop streams");
// stopStreams(); stopStreams();
// stopIMU(); // stopIMU();
delete[] rgb_buffer_; delete[] rgb_buffer_;
rgb_buffer_ = nullptr; rgb_buffer_ = nullptr;
@@ -119,8 +120,8 @@ void OBLidarNode::setupTopics() {
try { try {
getParameters(); getParameters();
setupDevices(); setupDevices();
// setupProfiles(); selectBaseStream();
// selectBaseStream(); setupProfiles();
setupPublishers(); setupPublishers();
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.getMessage()); RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.getMessage());
@@ -135,25 +136,45 @@ void OBLidarNode::setupTopics() {
} }
void OBLidarNode::getParameters() { void OBLidarNode::getParameters() {
setAndGetNodeParameter<std::string>(camera_name_, "camera_name", "camera"); setAndGetNodeParameter<std::string>(camera_name_, "camera_name", "lidar");
camera_link_frame_id_ = camera_name_ + "_link";
for (auto stream_index : IMAGE_STREAMS) { for (auto stream_index : IMAGE_STREAMS) {
std::string param_name; std::string param_name = stream_name_[stream_index] + "_format";
param_name = stream_name_[stream_index] + "_format";
setAndGetNodeParameter(format_str_[stream_index], param_name, format_str_[stream_index]); setAndGetNodeParameter(format_str_[stream_index], param_name, format_str_[stream_index]);
format_[stream_index] = OBFormatFromString(format_str_[stream_index]); format_[stream_index] = OBFormatFromString(format_str_[stream_index]);
RCLCPP_INFO_STREAM(logger_, "lidar format str: " << format_str_[stream_index]); param_name = stream_name_[stream_index] + "_rate";
RCLCPP_INFO_STREAM(logger_, "lidar format: " << format_[stream_index]); 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_"
<< magic_enum::enum_name(format_[LIDAR]));
} }
setAndGetNodeParameter<bool>(publish_tf_, "publish_tf", true); setAndGetNodeParameter<bool>(publish_tf_, "publish_tf", true);
setAndGetNodeParameter<double>(tf_publish_rate_, "tf_publish_rate", 0.0); setAndGetNodeParameter<double>(tf_publish_rate_, "tf_publish_rate", 0.0);
setAndGetNodeParameter<std::string>(time_domain_, "time_domain", "global"); setAndGetNodeParameter<std::string>(time_domain_, "time_domain", "global");
setAndGetNodeParameter<bool>(enable_heartbeat_, "enable_heartbeat", false); setAndGetNodeParameter<bool>(enable_heartbeat_, "enable_heartbeat", false);
setAndGetNodeParameter<std::string>(echo_mode_, "echo_mode", "single channel"); setAndGetNodeParameter<std::string>(echo_mode_, "echo_mode", "single channel");
setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default"); 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);
setAndGetNodeParameter<float>(max_range_, "max_range", 30.0);
} }
void OBLidarNode::setupDevices() { void OBLidarNode::setupDevices() {
auto sensor_list = device_->getSensorList();
for (size_t i = 0; i < sensor_list->getCount(); i++) {
auto sensor = sensor_list->getSensor(i);
auto profiles = sensor->getStreamProfileList();
for (size_t j = 0; j < profiles->getCount(); j++) {
auto profile = profiles->getProfile(j);
stream_index_pair sip{profile->getType(), 0};
if (sensors_.find(sip) != sensors_.end()) {
continue;
}
sensors_[sip] = sensor;
}
}
if (device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) { if (device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting heartbeat to " << (enable_heartbeat_ ? "ON" : "OFF")); RCLCPP_INFO_STREAM(logger_, "Setting heartbeat to " << (enable_heartbeat_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_);
@@ -171,164 +192,311 @@ void OBLidarNode::setupDevices() {
} }
} }
// void OBLidarNode::setupProfiles() { void OBLidarNode::setupProfiles() {
// // Image stream if (enable_stream_[LIDAR]) {
// for (const auto &elem : IMAGE_STREAMS) { const auto &sensor = sensors_[LIDAR];
// if (enable_stream_[elem]) { CHECK_NOTNULL(sensor.get());
// const auto &sensor = sensors_[elem]; auto profiles = sensor->getStreamProfileList();
// CHECK_NOTNULL(sensor.get()); CHECK_NOTNULL(profiles.get());
// auto profiles = sensor->getStreamProfileList(); CHECK(profiles->getCount() > 0);
// CHECK_NOTNULL(profiles.get()); for (size_t i = 0; i < profiles->getCount(); i++) {
// CHECK(profiles->getCount() > 0); auto base_profile = profiles->getProfile(i)->as<ob::LiDARStreamProfile>();
// for (size_t i = 0; i < profiles->getCount(); i++) { if (base_profile == nullptr) {
// auto base_profile = profiles->getProfile(i)->as<ob::VideoStreamProfile>(); throw std::runtime_error("Failed to get profile " + std::to_string(i));
// if (base_profile == nullptr) { }
// throw std::runtime_error("Failed to get profile " + std::to_string(i)); auto profile = base_profile->as<ob::LiDARStreamProfile>();
// } if (profile == nullptr) {
// auto profile = base_profile->as<ob::VideoStreamProfile>(); throw std::runtime_error("Failed cast profile to LiDARStreamProfile");
// if (profile == nullptr) { }
// throw std::runtime_error("Failed cast profile to VideoStreamProfile"); RCLCPP_DEBUG_STREAM(
// } logger_,
// RCLCPP_DEBUG_STREAM( "Sensor profile: " << "stream_type: " << magic_enum::enum_name(profile->getType())
// logger_, "Sensor profile: " << "Scan Rate: " << magic_enum::enum_name(profile->getScanRate())
// << "stream_type: " << magic_enum::enum_name(profile->getType()) << "Format:" << magic_enum::enum_name(profile->getFormat()));
// << "Format: " << profile->getFormat() << ", Width: " << supported_profiles_[LIDAR].emplace_back(profile);
// profile->getWidth() }
// << ", Height: " << profile->getHeight() << ", FPS: " << std::shared_ptr<ob::LiDARStreamProfile> selected_profile;
// profile->getFps()); std::shared_ptr<ob::LiDARStreamProfile> default_profile;
// supported_profiles_[elem].emplace_back(profile); try {
// } if (rate_[LIDAR] == OB_LIDAR_SCAN_UNKNOWN && format_[LIDAR] == OB_FORMAT_UNKNOWN) {
// std::shared_ptr<ob::VideoStreamProfile> selected_profile; selected_profile = profiles->getProfile(0)->as<ob::LiDARStreamProfile>();
// std::shared_ptr<ob::VideoStreamProfile> default_profile; } else {
// try { selected_profile = profiles->getLiDARStreamProfile(rate_[LIDAR], format_[LIDAR]);
// if (width_[elem] == 0 && height_[elem] == 0 && fps_[elem] == 0 && }
// format_[elem] == OB_FORMAT_UNKNOWN) {
// selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
// } else {
// selected_profile = profiles->getVideoStreamProfile(width_[elem], height_[elem],
// format_[elem], fps_[elem]);
// }
// } catch (const ob::Error &ex) { } catch (const ob::Error &ex) {
// RCLCPP_ERROR_STREAM( RCLCPP_ERROR_STREAM(
// logger_, "Failed to get " << stream_name_[elem] << " profile: " << ex.getMessage()); logger_, "Failed to get " << stream_name_[LIDAR] << " profile: " << ex.getMessage());
// RCLCPP_ERROR_STREAM( RCLCPP_ERROR_STREAM(logger_, "Stream: " << magic_enum::enum_name(LIDAR.first)
// logger_, "Stream: " << magic_enum::enum_name(elem.first) << ", Stream Index: " << LIDAR.second
// << ", Stream Index: " << elem.second << ", Width: " << << ", Scan Rate: " << rate_[LIDAR]
// width_[elem] << "Format:" << format_[LIDAR]);
// << ", Height: " << height_[elem] << ", FPS: " << fps_[elem] RCLCPP_INFO_STREAM(logger_, "Available profiles:");
// << ", Format: " << magic_enum::enum_name(format_[elem])); printSensorProfiles(sensor);
// RCLCPP_ERROR(logger_, RCLCPP_ERROR(logger_, "Because can not set this stream, so exit.");
// "Error: The device might be connected via USB 2.0. Please verify your " exit(-1);
// "configuration and try again. The current process will now exit."); }
// RCLCPP_INFO_STREAM(logger_, "Available profiles:"); if (!selected_profile) {
// printSensorProfiles(sensor); RCLCPP_WARN_STREAM(logger_, "Given stream configuration is not supported by the device! "
// RCLCPP_ERROR(logger_, "Because can not set this stream, so exit."); << " Stream: " << magic_enum::enum_name(LIDAR.first)
// exit(-1); << ", Stream Index: " << LIDAR.second
// } << ", Scan Rate: " << rate_[LIDAR]);
if (default_profile) {
RCLCPP_WARN_STREAM(logger_, "Using default profile instead.");
RCLCPP_WARN_STREAM(logger_, "default scan Rate "
<< magic_enum::enum_name(default_profile->getScanRate())
<< "default format:"
<< magic_enum::enum_name(default_profile->getFormat()));
selected_profile = default_profile;
} else {
RCLCPP_ERROR_STREAM(
logger_, " NO default_profile found , Stream: " << magic_enum::enum_name(LIDAR.first)
<< " will be disable");
enable_stream_[LIDAR] = false;
}
}
CHECK_NOTNULL(selected_profile);
stream_profile_[LIDAR] = selected_profile;
rate_[LIDAR] = selected_profile->getScanRate();
format_[LIDAR] = selected_profile->getFormat();
RCLCPP_INFO_STREAM(
logger_, " stream " << stream_name_[LIDAR] << " is enabled - scan rate: "
<< magic_enum::enum_name(selected_profile->getScanRate())
<< " format:" << magic_enum::enum_name(selected_profile->getFormat()));
}
}
// if (!selected_profile) { void OBLidarNode::selectBaseStream() {
// RCLCPP_WARN_STREAM(logger_, "Given stream configuration is not supported by the device! " enable_stream_[LIDAR] = true;
// << " Stream: " << magic_enum::enum_name(elem.first) if (enable_stream_[LIDAR]) {
// << ", Stream Index: " << elem.second base_stream_ = LIDAR;
// << ", Width: " << width_[elem] }
// << ", Height: " << height_[elem] << ", FPS: " << }
// fps_[elem]
// << ", Format: " << magic_enum::enum_name(format_[elem]));
// if (default_profile) {
// RCLCPP_WARN_STREAM(logger_, "Using default profile instead.");
// RCLCPP_WARN_STREAM(logger_, "default FPS " << default_profile->getFps());
// selected_profile = default_profile;
// } else {
// RCLCPP_ERROR_STREAM(
// logger_, " NO default_profile found , Stream: " <<
// magic_enum::enum_name(elem.first)
// << " will be disable");
// enable_stream_[elem] = false;
// continue;
// }
// }
// CHECK_NOTNULL(selected_profile);
// stream_profile_[elem] = selected_profile;
// height_[elem] = static_cast<int>(selected_profile->getHeight());
// width_[elem] = static_cast<int>(selected_profile->getWidth());
// fps_[elem] = static_cast<int>(selected_profile->getFps());
// format_[elem] = selected_profile->getFormat();
// updateImageConfig(elem);
// if (selected_profile->format() == OB_FORMAT_BGRA) {
// images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
// encoding_[elem] = sensor_msgs::image_encodings::BGRA8;
// unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
// } else if (selected_profile->format() == OB_FORMAT_RGBA) {
// images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
// encoding_[elem] = sensor_msgs::image_encodings::RGBA8;
// unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
// } else {
// images_[elem] =
// cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0));
// }
// RCLCPP_INFO_STREAM(logger_,
// " stream "
// << stream_name_[elem]
// << " is enabled - width: " << selected_profile->getWidth()
// << ", height: " << selected_profile->getHeight()
// << ", fps: " << selected_profile->getFps() << ", "
// << "Format: " <<
// magic_enum::enum_name(selected_profile->getFormat()));
// }
// }
// // IMU
// for (const auto &stream_index : HID_STREAMS) {
// if (!enable_stream_[stream_index]) {
// continue;
// }
// try {
// auto profile_list = sensors_[stream_index]->getStreamProfileList();
// if (stream_index == ACCEL) {
// auto full_scale_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
// auto sample_rate = sampleRateFromString(imu_rate_[stream_index]);
// auto profile = profile_list->getAccelStreamProfile(full_scale_range, sample_rate);
// stream_profile_[stream_index] = profile;
// } else if (stream_index == GYRO) {
// auto full_scale_range = fullGyroScaleRangeFromString(imu_range_[stream_index]);
// auto sample_rate = sampleRateFromString(imu_rate_[stream_index]);
// auto profile = profile_list->getGyroStreamProfile(full_scale_range, sample_rate);
// stream_profile_[stream_index] = profile;
// }
// RCLCPP_INFO_STREAM(logger_, "stream " << stream_name_[stream_index] << " full scale range "
// << imu_range_[stream_index] << " sample rate "
// << imu_rate_[stream_index]);
// } catch (const ob::Error &e) {
// RCLCPP_INFO_STREAM(logger_, "Failed to setup << " << stream_name_[stream_index]
// << " profile: " << e.getMessage());
// enable_stream_[stream_index] = false;
// stream_profile_[stream_index] = nullptr;
// }
// }
// }
// void OBLidarNode::selectBaseStream() { void OBLidarNode::printSensorProfiles(const std::shared_ptr<ob::Sensor> &sensor) {
// if (enable_stream_[DEPTH]) { auto profiles = sensor->getStreamProfileList();
// base_stream_ = DEPTH; for (size_t i = 0; i < profiles->getCount(); i++) {
// } else if (enable_stream_[INFRA0]) { auto origin_profile = profiles->getProfile(i);
// base_stream_ = INFRA0; if (sensor->getType() == OB_SENSOR_LIDAR) {
// } else if (enable_stream_[INFRA1]) { auto profile = origin_profile->as<ob::LiDARStreamProfile>();
// base_stream_ = INFRA1; RCLCPP_INFO_STREAM(logger_, "lidar scan rate: "
// } else if (enable_stream_[INFRA2]) { << profile->getScanRate()
// base_stream_ = INFRA2; << " format:" << magic_enum::enum_name(profile->getFormat()));
// } else if (enable_stream_[COLOR]) { } else {
// base_stream_ = COLOR; RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor->getType()));
// } }
// } }
}
void OBLidarNode::setupPublishers() { void OBLidarNode::setupPublishers() {
using PointCloud2 = sensor_msgs::msg::PointCloud2;
auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_); auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_);
if (use_intra_process_) { if (use_intra_process_) {
point_cloud_qos_profile = rmw_qos_profile_default; point_cloud_qos_profile = rmw_qos_profile_default;
} }
cloud_pub_ = node_->create_publisher<PointCloud2>( RCLCPP_INFO_STREAM(logger_, "rate_ " << magic_enum::enum_name(rate_[LIDAR]) << "format_"
<< magic_enum::enum_name(format_[LIDAR]));
if (format_[LIDAR] == OB_FORMAT_LIDAR_SCAN) {
scan_pub_ = node_->create_publisher<sensor_msgs::msg::LaserScan>(
"scan/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
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>(
"cloud/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile), "cloud/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
point_cloud_qos_profile)); point_cloud_qos_profile));
}
} }
void OBLidarNode::startStreams() {
if (pipeline_ != nullptr) {
pipeline_.reset();
}
pipeline_ = std::make_unique<ob::Pipeline>(device_);
try {
setupPipelineConfig();
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
onNewFrameSetCallback(frame_set);
});
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline: " << e.getMessage());
RCLCPP_INFO_STREAM(logger_, "try to disable ir stream and try again");
enable_stream_[LIDAR] = false;
setupPipelineConfig();
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
onNewFrameSetCallback(frame_set);
});
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline");
throw std::runtime_error("Failed to start pipeline");
}
pipeline_started_.store(true);
}
void OBLidarNode::stopStreams() {
if (!pipeline_started_ || !pipeline_) {
RCLCPP_INFO_STREAM(logger_, "pipeline not started or not exist, skip stop pipeline");
return;
}
try {
pipeline_->stop();
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << e.getMessage());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline");
}
}
void OBLidarNode::setupPipelineConfig() {
if (pipeline_config_) {
pipeline_config_.reset();
}
pipeline_config_ = std::make_shared<ob::Config>();
if (enable_stream_[LIDAR]) {
RCLCPP_INFO_STREAM(logger_, "Enable " << stream_name_[LIDAR] << " stream");
auto profile = stream_profile_[LIDAR]->as<ob::LiDARStreamProfile>();
if (enable_stream_[LIDAR]) {
auto video_profile = profile;
RCLCPP_INFO_STREAM(
logger_, "lidar profile: " << magic_enum::enum_name(video_profile->getScanRate()) << " "
<< magic_enum::enum_name(video_profile->getFormat()));
}
RCLCPP_INFO_STREAM(logger_,
"Stream " << stream_name_[LIDAR]
<< " scan rate: " << magic_enum::enum_name(profile->getScanRate())
<< " format: " << magic_enum::enum_name(profile->getFormat()));
pipeline_config_->enableStream(stream_profile_[LIDAR]);
}
}
void OBLidarNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set) {
if (!is_running_.load()) {
return;
}
if (!is_camera_node_initialized_.load()) {
return;
}
if (frame_set == nullptr) {
return;
}
try {
RCLCPP_INFO_ONCE(logger_, "New frame received");
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) {
publishPointCloud(frame_set);
}
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.getMessage());
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what());
} catch (...) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: unknown error");
}
}
void OBLidarNode::publishScan(std::shared_ptr<ob::FrameSet> frame_set) {
(void)frame_set;
if (frame_set == nullptr) {
return;
}
// std::shared_ptr<ob::LiDARPointsFrame> lidar_frame;
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->angle_min = 0.7853981852531433;
scan_msg->angle_max = 5.495169162750244;
scan_msg->angle_increment = 0.0026179938577115536;
scan_msg->time_increment = 1.0 / rate_int_[LIDAR] / scan_count;
scan_msg->scan_time = 1.0 / rate_int_[LIDAR];
scan_msg->range_min = min_range_;
scan_msg->range_max = max_range_;
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;
}
scan_msg->ranges[i] = scans_data[i].distance / 1000.0;
scan_msg->intensities[i] = scans_data[i].intensity;
}
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 ");
}
uint64_t OBLidarNode::getFrameTimestampUs(const std::shared_ptr<ob::Frame> &frame) {
if (frame == nullptr) {
RCLCPP_WARN(logger_, "getFrameTimestampUs: frame is nullptr, return 0");
return 0;
}
if (time_domain_ == "device") {
return frame->getTimeStampUs();
} else if (time_domain_ == "global") {
return frame->getGlobalTimeStampUs();
} else {
return frame->getSystemTimeStampUs();
}
}
void OBLidarNode::filterScan(sensor_msgs::msg::LaserScan &scan) {
double current_angle = scan.angle_min;
double max_angle = deg2rad(max_angle_);
double min_angle = deg2rad(min_angle_);
// map to 0 - 2 * M_PI
max_angle = std::fmod(max_angle + M_PI, 2 * M_PI);
if (max_angle < 0) {
max_angle += 2 * M_PI;
}
min_angle = std::fmod(min_angle + M_PI, 2 * M_PI);
if (min_angle < 0) {
min_angle += 2 * M_PI;
}
if (min_angle > max_angle) {
std::swap(min_angle, max_angle);
}
for (size_t i = 0; i < scan.ranges.size(); ++i, current_angle += scan.angle_increment) {
bool is_angle_in_range = (current_angle >= min_angle && current_angle <= max_angle);
bool is_range_in_range = (scan.ranges[i] >= min_range_ && scan.ranges[i] <= max_range_);
if (!(is_angle_in_range && is_range_in_range)) {
scan.ranges[i] = 0;
scan.intensities[i] = 0;
}
}
}
} // namespace orbbec_lidar
} // namespace orbbec_camera } // namespace orbbec_camera
orbbec_camera/src/utils.cpp
+29
View File
@@ -358,6 +358,25 @@ OBFormat OBFormatFromString(const std::string &format) {
} }
} }
OBLiDARScanRate OBScanRateFromInt(const int rate) {
std::cout << "OBScanRateFromInt: " << rate << std::endl;
if (rate == 5) {
return OB_LIDAR_SCAN_5HZ;
} else if (rate == 10) {
return OB_LIDAR_SCAN_10HZ;
} else if (rate == 15) {
return OB_LIDAR_SCAN_15HZ;
} else if (rate == 20) {
return OB_LIDAR_SCAN_20HZ;
} else if (rate == 25) {
return OB_LIDAR_SCAN_25HZ;
} else if (rate == 30) {
return OB_LIDAR_SCAN_30HZ;
} else {
return OB_LIDAR_SCAN_UNKNOWN;
}
}
std::string OBFormatToString(const OBFormat &format) { std::string OBFormatToString(const OBFormat &format) {
switch (format) { switch (format) {
case OB_FORMAT_MJPG: case OB_FORMAT_MJPG:
@@ -942,4 +961,14 @@ std::string getDistortionModels(OBCameraDistortion distortion) {
return sensor_msgs::distortion_models::PLUMB_BOB; return sensor_msgs::distortion_models::PLUMB_BOB;
} }
} }
double deg2rad(double deg) { return deg * M_PI / 180.0; }
double rad2deg(double rad) {
double angle_degrees = rad * (180.0 / M_PI);
if (angle_degrees < 0) {
angle_degrees += 360.0;
}
return angle_degrees;
}
} // namespace orbbec_camera } // namespace orbbec_camera
orbbec_camera/tools/list_camera_profile.cpp
+10 -4
View File
@@ -32,12 +32,18 @@ void listSensorProfiles(const std::shared_ptr<ob::Device>& device) {
<< magic_enum::enum_name(profile->getFormat()) << std::endl; << magic_enum::enum_name(profile->getFormat()) << std::endl;
} else if (sensor->getType() == OB_SENSOR_ACCEL) { } else if (sensor->getType() == OB_SENSOR_ACCEL) {
auto profile = origin_profile->as<ob::AccelStreamProfile>(); auto profile = origin_profile->as<ob::AccelStreamProfile>();
std::cout << magic_enum::enum_name(sensor->getType()) << " profile: " << profile->getSampleRate() std::cout << magic_enum::enum_name(sensor->getType())
<< " full scale_range " << profile->getFullScaleRange() << std::endl; << " profile: " << profile->getSampleRate() << " full scale_range "
<< profile->getFullScaleRange() << std::endl;
} else if (sensor->getType() == OB_SENSOR_GYRO) { } else if (sensor->getType() == OB_SENSOR_GYRO) {
auto profile = origin_profile->as<ob::GyroStreamProfile>(); auto profile = origin_profile->as<ob::GyroStreamProfile>();
std::cout << magic_enum::enum_name(sensor->getType()) << " profile: " << profile->getSampleRate() std::cout << magic_enum::enum_name(sensor->getType())
<< " full scale_range " << profile->getFullScaleRange() << std::endl; << " profile: " << profile->getSampleRate() << " full scale_range "
<< profile->getFullScaleRange() << std::endl;
} else if (sensor->getType() == OB_SENSOR_LIDAR) {
auto profile = origin_profile->as<ob::LiDARStreamProfile>();
std::cout << magic_enum::enum_name(sensor->getType())
<< " scan rate: " << magic_enum::enum_name(profile->getScanRate()) << " format:"<<magic_enum::enum_name(profile->getFormat())<< std::endl;
} else { } else {
std::cout << "Unknown profile: " << magic_enum::enum_name(sensor->getType()) << std::endl; std::cout << "Unknown profile: " << magic_enum::enum_name(sensor->getType()) << std::endl;
} }