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

Big refactoring code

Browse Source 
* Removal of unnecessary files
* Optimized multi-camera launch
* Explicitly list the parameters in the launch file
This commit is contained in:
默存
2023-02-06 17:26:10 +08:00
parent 04e4d4151e
commit 202719bd8c
59 changed files with 4931 additions and 806 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/magic_enum/magic_enum.hpp
+1538
View File
File diff suppressed because it is too large Load Diff
orbbec_camera/include/orbbec_camera/constants.h
+2 -2
View File
@@ -18,7 +18,7 @@
#define THREAD_NUM 4
#define OB_ROS_MAJOR_VERSION 1
#define OB_ROS_MINOR_VERSION 0
#define OB_ROS_MINOR_VERSION 1
#define OB_ROS_PATCH_VERSION 4
#ifndef STRINGIFY
@@ -48,7 +48,7 @@ const double DIAGNOSTICS_PERIOD = 0; // Static transform
const int IMAGE_WIDTH = 640;
const int IMAGE_HEIGHT = 480;
const double IMAGE_FPS = 30.0;
const int IMAGE_FPS = 30;
const std::string IMAGE_QOS = "SYSTEM_DEFAULT";
const std::string DEFAULT_QOS = "DEFAULT";
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+29 -22
View File
@@ -14,7 +14,6 @@
#include <glog/logging.h>
#include <nlohmann/json.hpp>
#include <magic_enum.hpp>
#include <memory>
#include <rclcpp/rclcpp.hpp>
@@ -32,6 +31,7 @@
#include <tf2/LinearMath/Quaternion.h>
#include <std_srvs/srv/set_bool.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <camera_info_manager/camera_info_manager.hpp>
#include <image_publisher/image_publisher.hpp>
#include <image_transport/publisher.hpp>
@@ -47,6 +47,7 @@
#include "orbbec_camera/constants.h"
#include "orbbec_camera/dynamic_params.h"
#include "magic_enum/magic_enum.hpp"
#define STREAM_NAME(sip) \
(static_cast<std::ostringstream&&>(std::ostringstream() \
@@ -123,6 +124,8 @@ class OBCameraNode {
void startPipeline();
void setupDefaultImageFormat();
void setupPublishers();
void publishStaticTF(const rclcpp::Time& t, const std::vector<float>& trans,
@@ -195,15 +198,16 @@ class OBCameraNode {
bool toggleSensor(const stream_index_pair& stream_index, bool enabled, std::string& msg);
void publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
void publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
void publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
void publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
void publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
void publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
void onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set);
void onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet>& frame_set);
void onNewFrameCallback(std::shared_ptr<ob::Frame> frame, const stream_index_pair& stream_index);
void onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
const stream_index_pair& stream_index);
bool setupFormatConvertType(OBFormat format);
@@ -221,19 +225,17 @@ class OBCameraNode {
std::map<stream_index_pair, OBCameraParam> ob_camera_param_;
std::map<stream_index_pair, int> width_;
std::map<stream_index_pair, int> height_;
std::map<stream_index_pair, double> 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> depth_aligned_frame_id_;
std::string camera_link_frame_id_;
bool depth_align_ = false;
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_;
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<ob_stream_type, int> image_format_;
std::map<stream_index_pair, int> image_format_;
std::map<stream_index_pair, std::vector<std::shared_ptr<ob::VideoStreamProfile>>>
enabled_profiles_;
std::map<stream_index_pair, uint32_t> seq_;
@@ -243,10 +245,8 @@ class OBCameraNode {
std::vector<int> compression_params_;
ob::FormatConvertFilter format_convert_filter_;
std::map<ob_frame_type, bool> is_first_frame_;
std::map<stream_index_pair, bool> enable_;
std::map<ob_stream_type, std::string> stream_name_;
std::map<stream_index_pair, bool> enable_stream_;
std::map<stream_index_pair, std::string> stream_name_;
std::map<stream_index_pair, image_transport::Publisher> image_publishers_;
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
camera_info_publishers_;
@@ -271,21 +271,28 @@ class OBCameraNode {
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
bool publish_tf_ = false;
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
std::shared_ptr<tf2_ros::TransformBroadcaster> dynamic_tf_broadcaster_;
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 colored_point_cloud_publisher_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_publisher_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr depth_point_cloud_publisher_;
bool enable_point_cloud_ = true;
bool enable_colored_point_cloud_ = false;
ob::PointCloudFilter point_cloud_filter_;
sensor_msgs::msg::PointCloud2 point_cloud_msg_;
rclcpp::Publisher<Extrinsics>::SharedPtr extrinsics_publisher_;
bool enable_publish_extrinsic_ = false;
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_;
};
} // namespace orbbec_camera
orbbec_camera/include/orbbec_camera/ob_camera_node_factory.h
+5 -10
View File
@@ -33,13 +33,13 @@ class OBCameraNodeFactory : public rclcpp::Node {
void startDevice(const std::shared_ptr<ob::DeviceList>& list);
void deviceConnectCallback(const std::shared_ptr<ob::DeviceList>& device_list);
void onDeviceConnected(const std::shared_ptr<ob::DeviceList>& device_list);
void deviceDisconnectCallback(const std::shared_ptr<ob::DeviceList>& device_list);
void onDeviceDisconnected(const std::shared_ptr<ob::DeviceList>& device_list);
static OBLogSeverity obLogSeverityFromString(const std::string& log_level);
static OBLogSeverity obLogSeverityFromString(const std::string_view& log_level);
void checkConnectTimer();
void checkConnectTimer() const;
void queryDevice();
@@ -51,16 +51,11 @@ class OBCameraNodeFactory : public rclcpp::Node {
std::shared_ptr<ob::DeviceInfo> device_info_ = nullptr;
std::atomic_bool is_alive_{false};
std::atomic_bool device_connected_{false};
std::string log_level_;
std::string serial_number_;
std::shared_ptr<Parameters> parameters_;
std::shared_ptr<Parameters> parameters_ = nullptr;
std::shared_ptr<std::thread> query_thread_ = nullptr;
std::recursive_mutex device_lock_;
size_t device_num_ = 1;
rclcpp::TimerBase::SharedPtr check_connect_timer_ = nullptr;
};
} // namespace orbbec_camera
#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::OBCameraNodeFactory)
orbbec_camera/include/orbbec_camera/utils.h
+3 -1
View File
@@ -13,7 +13,6 @@
#pragma once
#include <ostream>
#include <glog/logging.h>
#include <magic_enum.hpp>
#include <Eigen/Dense>
#include <tf2/LinearMath/Quaternion.h>
#include <rclcpp/rclcpp.hpp>
@@ -22,6 +21,7 @@
#include "sensor_msgs/distortion_models.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
#include "orbbec_camera_msgs/msg/extrinsics.hpp"
#include "magic_enum/magic_enum.hpp"
namespace orbbec_camera {
sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
@@ -46,4 +46,6 @@ OBFormat OBFormatFromString(const std::string& format);
std::string ObDeviceTypeToString(const OBDeviceType& type);
rmw_qos_profile_t getRMWQosProfileFromString(const std::string& str_qos);
} // namespace orbbec_camera