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

chore: Update launch file for gemini2RF

Browse Source
This commit is contained in:
Joe Dong
2024-05-08 20:59:49 +08:00
parent 7fcf3f899e
commit 6a809aba00
7 changed files with 187 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+2
View File
@@ -160,6 +160,8 @@ class OBCameraNode {
void setupProfiles();
void printSensorProfiles(const std::shared_ptr<ob::Sensor>& sensor);
void selectBaseStream();
void getParameters();
orbbec_camera/include/orbbec_camera/utils.h
+12
View File
@@ -56,6 +56,10 @@ std::string getObSDKVersion();
OBFormat OBFormatFromString(const std::string& format);
std::string OBFormatToString(const OBFormat& format);
std::ostream &operator<<(std::ostream &os, const OBFormat &rhs);
std::string ObDeviceTypeToString(const OBDeviceType& type);
rmw_qos_profile_t getRMWQosProfileFromString(const std::string& str_qos);
@@ -73,20 +77,28 @@ OB_SAMPLE_RATE sampleRateFromString(std::string& sample_rate);
std::string sampleRateToString(const OB_SAMPLE_RATE& sample_rate);
std::ostream &operator<<(std::ostream &os, const OB_SAMPLE_RATE &rhs);
OB_GYRO_FULL_SCALE_RANGE fullGyroScaleRangeFromString(std::string& full_scale_range);
std::string fullGyroScaleRangeToString(const OB_GYRO_FULL_SCALE_RANGE& full_scale_range);
std::ostream &operator<<(std::ostream &os, const OB_GYRO_FULL_SCALE_RANGE &rhs);
OBAccelFullScaleRange fullAccelScaleRangeFromString(std::string& full_scale_range);
std::string fullAccelScaleRangeToString(const OBAccelFullScaleRange& full_scale_range);
std::ostream &operator<<(std::ostream &os, const OBAccelFullScaleRange &rhs);
std::string parseUsbPort(const std::string& line);
bool isValidJPEG(const std::shared_ptr<ob::ColorFrame>& frame);
std::string metaDataTypeToString(const OBFrameMetadataType& meta_data_type);
std::ostream &operator<<(std::ostream &os, const OBFrameMetadataType &rhs);
OBHoleFillingMode holeFillingModeFromString(const std::string& hole_filling_mode);
bool isGemini2R(int pid);
orbbec_camera/launch/gemini2R.launch.py
+4 -4
View File
@@ -23,7 +23,7 @@ def generate_launch_description():
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('color_width', default_value='1280'),
DeclareLaunchArgument('color_height', default_value='720'),
DeclareLaunchArgument('color_fps', default_value='10'),
DeclareLaunchArgument('color_fps', default_value='15'),
DeclareLaunchArgument('color_format', default_value='MJPG'),
DeclareLaunchArgument('enable_color', default_value='true'),
DeclareLaunchArgument('color_qos', default_value='default'),
@@ -31,21 +31,21 @@ def generate_launch_description():
DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
DeclareLaunchArgument('depth_width', default_value='848'),
DeclareLaunchArgument('depth_height', default_value='480'),
DeclareLaunchArgument('depth_fps', default_value='10'),
DeclareLaunchArgument('depth_fps', default_value='15'),
DeclareLaunchArgument('depth_format', default_value='Y16'),
DeclareLaunchArgument('enable_depth', default_value='true'),
DeclareLaunchArgument('depth_qos', default_value='default'),
DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
DeclareLaunchArgument('left_ir_width', default_value='848'),
DeclareLaunchArgument('left_ir_height', default_value='480'),
DeclareLaunchArgument('left_ir_fps', default_value='10'),
DeclareLaunchArgument('left_ir_fps', default_value='15'),
DeclareLaunchArgument('left_ir_format', default_value='Y8'),
DeclareLaunchArgument('enable_left_ir', default_value='true'),
DeclareLaunchArgument('left_ir_qos', default_value='default'),
DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'),
DeclareLaunchArgument('right_ir_width', default_value='848'),
DeclareLaunchArgument('right_ir_height', default_value='480'),
DeclareLaunchArgument('right_ir_fps', default_value='10'),
DeclareLaunchArgument('right_ir_fps', default_value='15'),
DeclareLaunchArgument('right_ir_format', default_value='Y8'),
DeclareLaunchArgument('enable_right_ir', default_value='true'),
DeclareLaunchArgument('right_ir_qos', default_value='default'),
orbbec_camera/launch/gemini2RF.launch.py
-162
View File
@@ -1,162 +0,0 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import PushRosNamespace
from launch.actions import GroupAction
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch_ros.actions import Node
import os
def generate_launch_description():
# Declare arguments
args = [
DeclareLaunchArgument('camera_name', default_value='camera'),
DeclareLaunchArgument('depth_registration', default_value='true'),
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='true'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('color_width', default_value='640'),
DeclareLaunchArgument('color_height', default_value='480'),
DeclareLaunchArgument('color_fps', default_value='30'),
DeclareLaunchArgument('color_format', default_value='MJPG'),
DeclareLaunchArgument('enable_color', default_value='true'),
DeclareLaunchArgument('color_qos', default_value='default'),
DeclareLaunchArgument('color_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
DeclareLaunchArgument('depth_width', default_value='640'),
DeclareLaunchArgument('depth_height', default_value='480'),
DeclareLaunchArgument('depth_fps', default_value='30'),
DeclareLaunchArgument('depth_format', default_value='Y16'),
DeclareLaunchArgument('enable_depth', default_value='true'),
DeclareLaunchArgument('depth_qos', default_value='default'),
DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
DeclareLaunchArgument('left_ir_width', default_value='640'),
DeclareLaunchArgument('left_ir_height', default_value='480'),
DeclareLaunchArgument('left_ir_fps', default_value='30'),
DeclareLaunchArgument('left_ir_format', default_value='Y8'),
DeclareLaunchArgument('enable_left_ir', default_value='false'),
DeclareLaunchArgument('left_ir_qos', default_value='default'),
DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'),
DeclareLaunchArgument('right_ir_width', default_value='640'),
DeclareLaunchArgument('right_ir_height', default_value='480'),
DeclareLaunchArgument('right_ir_fps', default_value='30'),
DeclareLaunchArgument('right_ir_format', default_value='Y8'),
DeclareLaunchArgument('enable_right_ir', default_value='false'),
DeclareLaunchArgument('right_ir_qos', default_value='default'),
DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'),
DeclareLaunchArgument('enable_accel', default_value='false'),
DeclareLaunchArgument('accel_rate', default_value='200hz'),
DeclareLaunchArgument('accel_range', default_value='4g'),
DeclareLaunchArgument('enable_gyro', default_value='false'),
DeclareLaunchArgument('gyro_rate', default_value='200hz'),
DeclareLaunchArgument('gyro_range', default_value='1000dps'),
DeclareLaunchArgument('liner_accel_cov', default_value='0.01'),
DeclareLaunchArgument('angular_vel_cov', default_value='0.01'),
DeclareLaunchArgument('publish_tf', default_value='true'),
DeclareLaunchArgument('tf_publish_rate', default_value='10.0'),
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
DeclareLaunchArgument('sync_mode', default_value='free_run'),
DeclareLaunchArgument('depth_delay_us', default_value='0'),
DeclareLaunchArgument('color_delay_us', default_value='0'),
DeclareLaunchArgument('trigger2image_delay_us', default_value='0'),
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='true'),
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
DeclareLaunchArgument('enable_decimation_filter', default_value='false'),
DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
DeclareLaunchArgument('enable_sequence_id_filter', default_value='false'),
DeclareLaunchArgument('enable_threshold_filter', default_value='false'),
DeclareLaunchArgument('enable_noise_removal_filter', default_value='true'),
DeclareLaunchArgument('enable_spatial_filter', default_value='false'),
DeclareLaunchArgument('enable_temporal_filter', default_value='false'),
DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'),
DeclareLaunchArgument('decimation_filter_scale_range', default_value='2'),
DeclareLaunchArgument('sequence_id_filter_id', default_value='1'),
DeclareLaunchArgument('threshold_filter_max', default_value='16000'),
DeclareLaunchArgument('threshold_filter_min', default_value='0'),
DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='8'),
DeclareLaunchArgument('noise_removal_filter_max_size', default_value='80'),
DeclareLaunchArgument('spatial_filter_alpha', default_value='0.5'),
DeclareLaunchArgument('spatial_filter_diff_threshold', default_value='8'),
DeclareLaunchArgument('spatial_filter_magnitude', default_value='1'),
DeclareLaunchArgument('spatial_filter_radius', default_value='1'),
DeclareLaunchArgument('temporal_filter_diff_threshold', default_value='0.1'),
DeclareLaunchArgument('temporal_filter_weight', default_value='0.4'),
DeclareLaunchArgument('hole_filling_filter_mode', default_value='FILL_TOP'),
DeclareLaunchArgument('align_mode', default_value='SW'),
DeclareLaunchArgument('diagnostic_period', default_value='1.0'),
DeclareLaunchArgument('enable_laser', default_value='true'),
DeclareLaunchArgument('depth_precision', default_value='1mm'),
# Laser on/off alternate mode, 0: off, 1: on-off alternate, 2: off-on alternate.
DeclareLaunchArgument('laser_on_off_mode', default_value='0'),
DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
]
# Node configuration
parameters = [{arg.name: LaunchConfiguration(arg.name)} for arg in args]
# get ROS_DISTRO
ros_distro = os.environ["ROS_DISTRO"]
if ros_distro == "foxy":
return LaunchDescription(
args
+ [
Node(
package="orbbec_camera",
executable="orbbec_camera_node",
name="ob_camera_node",
namespace=LaunchConfiguration("camera_name"),
parameters=parameters,
output="screen",
)
]
)
# Define the ComposableNode
else:
# Define the ComposableNode
compose_node = ComposableNode(
package="orbbec_camera",
plugin="orbbec_camera::OBCameraNodeDriver",
name=LaunchConfiguration("camera_name"),
namespace="",
parameters=parameters,
)
# Define the ComposableNodeContainer
container = ComposableNodeContainer(
name="camera_container",
namespace="",
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[
compose_node,
],
output="screen",
)
# Launch description
ld = LaunchDescription(
args
+ [
GroupAction(
[PushRosNamespace(LaunchConfiguration("camera_name")), container]
)
]
)
return ld
orbbec_camera/src/ob_camera_node.cpp
+44 -2
View File
@@ -333,6 +333,42 @@ void OBCameraNode::selectBaseStream() {
base_stream_ = COLOR;
}
}
void OBCameraNode::printSensorProfiles(const std::shared_ptr<ob::Sensor> &sensor) {
auto profiles = sensor->getStreamProfileList();
for (size_t i = 0; i < profiles->count(); i++) {
auto origin_profile = profiles->getProfile(i);
if (sensor->type() == OB_SENSOR_COLOR) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "color profile: " << profile->width() << "x" << profile->height()
<< " " << profile->fps() << "fps "
<< profile->format());
} else if (sensor->type() == OB_SENSOR_DEPTH) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "depth profile: " << profile->width() << "x" << profile->height()
<< " " << profile->fps() << "fps "
<< profile->format());
} else if (sensor->type() == OB_SENSOR_IR) {
auto profile = origin_profile->as<ob::VideoStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "ir profile: " << profile->width() << "x" << profile->height()
<< " " << profile->fps() << "fps "
<< profile->format());
} else if (sensor->type() == OB_SENSOR_ACCEL) {
auto profile = origin_profile->as<ob::AccelStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "accel profile: sampleRate " << profile->sampleRate()
<< " full scale_range "
<< profile->fullScaleRange());
} else if (sensor->type() == OB_SENSOR_GYRO) {
auto profile = origin_profile->as<ob::GyroStreamProfile>();
RCLCPP_INFO_STREAM(logger_, "gyro profile: sampleRate " << profile->sampleRate()
<< " full scale_range "
<< profile->fullScaleRange());
} else {
RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor->type()));
}
}
}
void OBCameraNode::setupProfiles() {
// Image stream
for (const auto &elem : IMAGE_STREAMS) {
@@ -360,12 +396,17 @@ void OBCameraNode::setupProfiles() {
profiles->getVideoStreamProfile(width_[elem], height_[elem], format_[elem]);
} catch (const ob::Error &ex) {
RCLCPP_ERROR_STREAM(
logger_, "Failed to get " << stream_name_[elem] << " << profile: " << ex.getMessage());
logger_, "Failed to get " << stream_name_[elem] << " profile: " << ex.getMessage());
RCLCPP_ERROR_STREAM(
logger_, "Stream: " << magic_enum::enum_name(elem.first)
<< ", Stream Index: " << elem.second << ", Width: " << width_[elem]
<< ", Height: " << height_[elem] << ", FPS: " << fps_[elem]
<< ", Format: " << magic_enum::enum_name(format_[elem]));
RCLCPP_ERROR(logger_,
"Error: The device might be connected via USB 2.0. Please verify your "
"configuration and try again. The current process will now exit.");
RCLCPP_INFO_STREAM(logger_, "Available profiles:");
printSensorProfiles(sensor);
exit(-1);
}
@@ -745,7 +786,8 @@ void OBCameraNode::getParameters() {
std::string align_target_stream_str_;
setAndGetNodeParameter<std::string>(align_target_stream_str_, "align_target_stream", "COLOR");
align_target_stream_ = obStreamTypeFromString(align_target_stream_str_);
setAndGetNodeParameter<bool>(retry_on_usb3_detection_failure_, "retry_on_usb3_detection_failure", false);
setAndGetNodeParameter<bool>(retry_on_usb3_detection_failure_, "retry_on_usb3_detection_failure",
false);
}
void OBCameraNode::setupTopics() {
orbbec_camera/src/utils.cpp
+121
View File
@@ -313,11 +313,111 @@ OBFormat OBFormatFromString(const std::string &format) {
return OB_FORMAT_BGR;
} else if (fixed_format == "Y14") {
return OB_FORMAT_Y14;
} else if (fixed_format == "BGRA") {
return OB_FORMAT_BGRA;
} else if (fixed_format == "COMPRESSED") {
return OB_FORMAT_COMPRESSED;
} else if (fixed_format == "RVL") {
return OB_FORMAT_RVL;
} else if (fixed_format == "Z16") {
return OB_FORMAT_Z16;
} else if (fixed_format == "YV12") {
return OB_FORMAT_YV12;
} else if (fixed_format == "BA81") {
return OB_FORMAT_BA81;
} else if (fixed_format == "RGBA") {
return OB_FORMAT_RGBA;
} else if (fixed_format == "BYR2") {
return OB_FORMAT_BYR2;
} else if (fixed_format == "RW16") {
return OB_FORMAT_RW16;
} else if (fixed_format == "DISP16") {
return OB_FORMAT_DISP16;
} else {
return OB_FORMAT_UNKNOWN;
}
}
std::string OBFormatToString(const OBFormat &format) {
switch (format) {
case OB_FORMAT_MJPG:
return "MJPG";
case OB_FORMAT_YUYV:
return "YUYV";
case OB_FORMAT_YUY2:
return "YUYV2";
case OB_FORMAT_UYVY:
return "UYVY";
case OB_FORMAT_NV12:
return "NV12";
case OB_FORMAT_NV21:
return "NV21";
case OB_FORMAT_H264:
return "H264";
case OB_FORMAT_H265:
return "H265";
case OB_FORMAT_Y16:
return "Y16";
case OB_FORMAT_Y8:
return "Y8";
case OB_FORMAT_Y10:
return "Y10";
case OB_FORMAT_Y11:
return "Y11";
case OB_FORMAT_Y12:
return "Y12";
case OB_FORMAT_GRAY:
return "GRAY";
case OB_FORMAT_HEVC:
return "HEVC";
case OB_FORMAT_I420:
return "I420";
case OB_FORMAT_ACCEL:
return "ACCEL";
case OB_FORMAT_GYRO:
return "GYRO";
case OB_FORMAT_POINT:
return "POINT";
case OB_FORMAT_RGB_POINT:
return "RGB_POINT";
case OB_FORMAT_RLE:
return "REL";
case OB_FORMAT_RGB888:
return "RGB888";
case OB_FORMAT_BGR:
return "BGR";
case OB_FORMAT_Y14:
return "Y14";
case OB_FORMAT_BGRA:
return "BGRA";
case OB_FORMAT_COMPRESSED:
return "COMPRESSED";
case OB_FORMAT_RVL:
return "RVL";
case OB_FORMAT_Z16:
return "Z16";
case OB_FORMAT_YV12:
return "YV12";
case OB_FORMAT_BA81:
return "BA81";
case OB_FORMAT_RGBA:
return "RGBA";
case OB_FORMAT_BYR2:
return "BYR2";
case OB_FORMAT_RW16:
return "RW16";
case OB_FORMAT_DISP16:
return "DISP16";
default:
return "UNKNOWN";
}
}
std::ostream &operator<<(std::ostream &os, const OBFormat &rhs) {
os << OBFormatToString(rhs);
return os;
}
std::string ObDeviceTypeToString(const OBDeviceType &type) {
switch (type) {
case OBDeviceType::OB_STRUCTURED_LIGHT_BINOCULAR_CAMERA:
@@ -490,6 +590,11 @@ std::string sampleRateToString(const OB_SAMPLE_RATE &sample_rate) {
}
}
std::ostream &operator<<(std::ostream &os, const OB_SAMPLE_RATE &rhs) {
os << sampleRateToString(rhs);
return os;
}
OB_GYRO_FULL_SCALE_RANGE fullGyroScaleRangeFromString(std::string &full_scale_range) {
std::transform(full_scale_range.begin(), full_scale_range.end(), full_scale_range.begin(),
::tolower);
@@ -539,6 +644,11 @@ std::string fullGyroScaleRangeToString(const OB_GYRO_FULL_SCALE_RANGE &full_scal
}
}
std::ostream &operator<<(std::ostream &os, const OB_GYRO_FULL_SCALE_RANGE &rhs) {
os << fullGyroScaleRangeToString(rhs);
return os;
}
OBAccelFullScaleRange fullAccelScaleRangeFromString(std::string &full_scale_range) {
std::transform(full_scale_range.begin(), full_scale_range.end(), full_scale_range.begin(),
::tolower);
@@ -572,6 +682,11 @@ std::string fullAccelScaleRangeToString(const OBAccelFullScaleRange &full_scale_
}
}
std::ostream &operator<<(std::ostream &os, const OBAccelFullScaleRange &rhs) {
os << fullAccelScaleRangeToString(rhs);
return os;
}
std::string parseUsbPort(const std::string &line) {
std::string port_id;
std::regex self_regex("(?:[^ ]+/usb[0-9]+[0-9./-]*/){0,1}([0-9.-]+)(:){0,1}[^ ]*",
@@ -676,6 +791,12 @@ std::string metaDataTypeToString(const OBFrameMetadataType &meta_data_type) {
return "unknown_field";
}
}
std::ostream &operator<<(std::ostream &os, const OBFrameMetadataType &rhs) {
os << metaDataTypeToString(rhs);
return os;
}
OBHoleFillingMode holeFillingModeFromString(const std::string &hole_filling_mode) {
if (hole_filling_mode == "FILL_TOP") {
return OB_HOLE_FILL_TOP;