i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-13 03:30:18 +08:00
Code Issues Packages Projects Releases Wiki Activity

clean code

Browse Source
This commit is contained in:
Joe Dong
2022-06-09 13:52:42 +08:00
parent 66ed074c55
commit 91e1e0ae5b
6 changed files with 47 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/constants.h
+2 -2
View File
@@ -43,7 +43,7 @@ const double DIAGNOSTICS_PERIOD = 0; // Static transform
const int IMAGE_WIDTH = 640;
const int IMAGE_HEIGHT = 480;
const double IMAGE_FPS = 30;
const double IMAGE_FPS = 30.0;
const std::string IMAGE_QOS = "SYSTEM_DEFAULT";
const std::string DEFAULT_QOS = "DEFAULT";
@@ -88,6 +88,6 @@ const std::string DEFAULT_ALIGNED_DEPTH_TO_FISHEYE_FRAME_ID =
const std::string DEFAULT_UNITE_IMU_METHOD = "";
const std::string DEFAULT_FILTERS = "";
const std::string DEFAULT_TOPIC_ODOM_IN = "";
const std::string DEFAULT_D2C_MODE = "sw"; // sw = software mode, hw=hardware mode, none,
const float ROS_DEPTH_SCALE = 0.001;
} // namespace orbbec_camera
orbbec_camera/include/orbbec_camera/dynamic_params.h
+1 -1
View File
@@ -19,7 +19,7 @@ namespace orbbec_camera {
class Parameters {
public:
explicit Parameters(rclcpp::Node* node);
~Parameters();
~Parameters() noexcept;
rclcpp::ParameterValue setParam(const std::string& param_name,
rclcpp::ParameterValue initial_value,
const std::function<void(const rclcpp::Parameter&)>& func =
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+4 -4
View File
@@ -97,7 +97,7 @@ class OBCameraNode {
std::shared_ptr<Parameters> parameters);
template <class T>
void setNgetNodeParameter(
void setAndGetNodeParameter(
T& param, const std::string& param_name, const T& default_value,
const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor =
rcl_interfaces::msg::ParameterDescriptor()); // set and get parameter
@@ -192,7 +192,7 @@ class OBCameraNode {
void publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
void publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_set, const rclcpp::Time& t);
void publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
void publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
@@ -224,6 +224,8 @@ class OBCameraNode {
std::map<stream_index_pair, std::string> depth_aligned_frame_id_;
std::string base_frame_id_;
bool align_depth_;
bool publish_rgb_point_cloud_;
std::string d2c_mode_; // sw, hw, none
std::map<stream_index_pair, std::string> qos_;
std::map<stream_index_pair, std::string> info_qos_;
std::map<stream_index_pair, ob_format> format_;
@@ -263,7 +265,6 @@ class OBCameraNode {
sensor_msgs::msg::PointCloud2 point_cloud_msg_;
rclcpp::Publisher<Extrinsics>::SharedPtr extrinsics_publisher_;
OBD2CTransform extrinsics_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
orbbec_camera_msgs::msg::DeviceInfo device_info_;
rclcpp::Service<GetDeviceInfo>::SharedPtr get_device_srv_;
@@ -272,7 +273,6 @@ class OBCameraNode {
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_floor_enable_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_fan_mode_srv_;
std::vector<geometry_msgs::msg::TransformStamped> static_tf_msgs_;
std::mutex tf_lock_;
std::shared_ptr<std::thread> tf_thread_;
std::condition_variable tf_cv_;
double tf_publish_rate_;
orbbec_camera/params/orbbec_camera_params.yaml
+2
View File
@@ -21,3 +21,5 @@
publish_tf: true
align_depth: true
tf_publish_rate: 10.0
publish_rgb_point_cloud : false
d2c_mode : "hw"
orbbec_camera/src/dynamic_params.cpp
+6 -2
View File
@@ -36,9 +36,13 @@ Parameters::Parameters(rclcpp::Node *node)
});
}
Parameters::~Parameters() {
Parameters::~Parameters() noexcept {
for (auto const &param : param_functions_) {
node_->undeclare_parameter(param.first);
try {
node_->undeclare_parameter(param.first);
} catch (const rclcpp::exceptions::InvalidParameterTypeException &e) {
RCLCPP_ERROR_STREAM(logger_, e.what());
}
}
}
orbbec_camera/src/ob_camera_node.cpp
+32 -45
View File
@@ -52,7 +52,7 @@ OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> devic
}
template <class T>
void OBCameraNode::setNgetNodeParameter(
void OBCameraNode::setAndGetNodeParameter(
T& param, const std::string& param_name, const T& default_value,
const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor) {
try {
@@ -106,7 +106,6 @@ void OBCameraNode::setupDevices() {
void OBCameraNode::setupProfiles() {
config_ = std::make_shared<ob::Config>();
config_->setAlignMode(ALIGN_D2C_HW_MODE);
for (const auto& elem : IMAGE_STREAMS) {
if (enable_[elem]) {
const auto& sensor = sensors_[elem];
@@ -155,6 +154,13 @@ void OBCameraNode::setupProfiles() {
}
void OBCameraNode::startPipeline() {
if (d2c_mode_ == "sw") {
config_->setAlignMode(ALIGN_D2C_SW_MODE);
} else if (d2c_mode_ == "hw") {
config_->setAlignMode(ALIGN_D2C_HW_MODE);
} else {
config_->setAlignMode(ALIGN_DISABLE);
}
pipeline_ = std::make_unique<ob::Pipeline>(device_);
pipeline_->start(config_, [this](std::shared_ptr<ob::FrameSet> frame_set) {
frameSetCallback(std::move(frame_set));
@@ -164,25 +170,27 @@ void OBCameraNode::startPipeline() {
void OBCameraNode::getParameters() {
for (auto stream_index : IMAGE_STREAMS) {
std::string param_name = stream_name_[stream_index.first] + "_width";
setNgetNodeParameter(width_[stream_index], param_name, IMAGE_WIDTH);
setAndGetNodeParameter(width_[stream_index], param_name, IMAGE_WIDTH);
param_name = stream_name_[stream_index.first] + "_height";
setNgetNodeParameter(height_[stream_index], param_name, IMAGE_HEIGHT);
setAndGetNodeParameter(height_[stream_index], param_name, IMAGE_HEIGHT);
param_name = stream_name_[stream_index.first] + "_fps";
setNgetNodeParameter(fps_[stream_index], param_name, IMAGE_FPS);
setAndGetNodeParameter(fps_[stream_index], param_name, IMAGE_FPS);
param_name = "enable_" + stream_name_[stream_index.first];
setNgetNodeParameter(enable_[stream_index], param_name, true);
setAndGetNodeParameter(enable_[stream_index], param_name, true);
param_name = stream_name_[stream_index.first] + "_frame_id";
std::string default_frame_id = "camera_" + stream_name_[stream_index.first] + "_frame";
setNgetNodeParameter(frame_id_[stream_index], param_name, default_frame_id);
setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id);
std::string default_optical_frame_id =
"camera_" + stream_name_[stream_index.first] + "_optical_frame";
param_name = stream_name_[stream_index.first] + "_optical_frame_id";
setNgetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
depth_aligned_frame_id_[stream_index] = stream_name_[OB_STREAM_COLOR] + "_optical_frame";
}
setNgetNodeParameter(publish_tf_, "publish_tf", true);
setNgetNodeParameter(align_depth_, "align_depth", true);
setNgetNodeParameter(tf_publish_rate_, "tf_publish_rate", 10.0);
setAndGetNodeParameter(publish_tf_, "publish_tf", true);
setAndGetNodeParameter(align_depth_, "align_depth", true);
setAndGetNodeParameter(tf_publish_rate_, "tf_publish_rate", 10.0);
setAndGetNodeParameter(publish_rgb_point_cloud_, "publish_rgb_point_cloud", false);
setAndGetNodeParameter(d2c_mode_, "d2c_mode_", DEFAULT_D2C_MODE);
}
void OBCameraNode::setupTopics() {
@@ -214,35 +222,19 @@ void OBCameraNode::setupPublishers() {
}
void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
#if 0
// NOTE: This block code only for debug, it will be crazy slowly
static int cnt = 0;
const std::string home_dir = std::getenv("HOME");
const std::string pc_file_name = home_dir + "/pc/point_cloud.ply";
if ((++cnt) % 20 == 0) {
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
RCLCPP_INFO_STREAM(logger_, "has rgb pc");
auto depth_frame = frame_set->depthFrame();
auto color_frame = frame_set->colorFrame();
auto camera_param = findCameraParam(color_frame->width(), color_frame->height(),
depth_frame->width(), depth_frame->height());
point_cloud_filter_.setCameraParam(*camera_param);
point_cloud_filter_.setCreatePointFormat(OB_FORMAT_RGB_POINT);
auto frame = point_cloud_filter_.process(frame_set);
saveRGBPointsToPly(frame, pc_file_name);
}
}
#endif
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
auto start = rclcpp::Clock().now();
if (publish_rgb_point_cloud_ && frame_set->depthFrame() != nullptr &&
frame_set->colorFrame() != nullptr) {
publishColorPointCloud(frame_set);
auto end = rclcpp::Clock().now();
} else if (frame_set->depthFrame() != nullptr) {
publishDepthPointCloud(frame_set);
}
}
void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_set,
const rclcpp::Time& t) {
void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
auto camera_param = pipeline_->getCameraParam();
point_cloud_filter_.setCameraParam(camera_param);
point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
auto depth_frame = frame_set->depthFrame();
auto frame = point_cloud_filter_.process(frame_set);
size_t point_size = frame->dataSize() / sizeof(OBPoint);
auto* points = (OBPoint*)frame->data();
@@ -250,13 +242,8 @@ void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_se
sensor_msgs::PointCloud2Modifier modifier(point_cloud_msg_);
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.resize(point_size);
point_cloud_msg_.width = width_[DEPTH];
point_cloud_msg_.height = height_[DEPTH];
std::string format_str = "intensity";
point_cloud_msg_.point_step =
addPointField(point_cloud_msg_, format_str.c_str(), 1, sensor_msgs::msg::PointField::FLOAT32,
point_cloud_msg_.point_step);
point_cloud_msg_.width = depth_frame->width();
point_cloud_msg_.height = depth_frame->height();
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");
@@ -269,15 +256,15 @@ void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_se
*iter_x = static_cast<float>(points->x / 1000.0);
*iter_y = static_cast<float>(points->y / 1000.0);
*iter_z = static_cast<float>(points->z / 1000.0);
++iter_x;
++iter_y;
++iter_z;
++valid_count;
}
}
point_cloud_msg_.header.stamp = t;
point_cloud_msg_.header.frame_id = optical_frame_id_[COLOR];
auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp());
point_cloud_msg_.header.stamp = timestamp;
point_cloud_msg_.header.frame_id = optical_frame_id_[DEPTH];
point_cloud_publisher_->publish(point_cloud_msg_);
}