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

Add rotation, flip and mirror parameters and services

Browse Source
This commit is contained in:
jj
2025-05-12 11:52:56 +08:00
parent b2ef7d2d07
commit ba9c54b728
12 changed files with 1103 additions and 933 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+42 -5
View File
@@ -325,17 +325,50 @@ void OBCameraNode::setupDevices() {
mirrorPropertyID = OB_PROP_DEPTH_MIRROR_BOOL;
} else if (stream_index == INFRA0) {
mirrorPropertyID = OB_PROP_IR_MIRROR_BOOL;
} else if (stream_index == INFRA1) {
mirrorPropertyID = OB_PROP_IR_MIRROR_BOOL;
} else if (stream_index == INFRA2) {
mirrorPropertyID = OB_PROP_IR_RIGHT_MIRROR_BOOL;
}
if (device_->isPropertySupported(mirrorPropertyID, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting " << stream_name_[stream_index] << " mirror to "
<< (mirror_stream_[stream_index] ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, mirrorPropertyID, mirror_stream_[stream_index]);
}
OBPropertyID flipPropertyID = OB_PROP_DEPTH_FLIP_BOOL;
if (stream_index == COLOR) {
flipPropertyID = OB_PROP_COLOR_FLIP_BOOL;
} else if (stream_index == DEPTH) {
flipPropertyID = OB_PROP_DEPTH_FLIP_BOOL;
} else if (stream_index == INFRA0) {
flipPropertyID = OB_PROP_IR_FLIP_BOOL;
} else if (stream_index == INFRA1) {
flipPropertyID = OB_PROP_IR_FLIP_BOOL;
} else if (stream_index == INFRA2) {
flipPropertyID = OB_PROP_IR_RIGHT_FLIP_BOOL;
}
if (device_->isPropertySupported(flipPropertyID, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting " << stream_name_[stream_index] << " flip to "
<< (flip_stream_[stream_index] ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, mirrorPropertyID, flip_stream_[stream_index]);
TRY_TO_SET_PROPERTY(setBoolProperty, flipPropertyID, flip_stream_[stream_index]);
}
OBPropertyID rotationPropertyID = OB_PROP_DEPTH_ROTATE_INT;
if (stream_index == COLOR) {
rotationPropertyID = OB_PROP_COLOR_ROTATE_INT;
} else if (stream_index == DEPTH) {
rotationPropertyID = OB_PROP_DEPTH_ROTATE_INT;
} else if (stream_index == INFRA0) {
rotationPropertyID = OB_PROP_IR_ROTATE_INT;
} else if (stream_index == INFRA1) {
rotationPropertyID = OB_PROP_IR_ROTATE_INT;
} else if (stream_index == INFRA2) {
rotationPropertyID = OB_PROP_IR_RIGHT_ROTATE_INT;
}
if (rotation_stream_[stream_index] != -1 &&
device_->isPropertySupported(rotationPropertyID, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting " << stream_name_[stream_index] << " rotation to "
<< rotation_stream_[stream_index]);
TRY_TO_SET_PROPERTY(setIntProperty, rotationPropertyID, rotation_stream_[stream_index]);
}
}
}
@@ -1459,8 +1492,12 @@ void OBCameraNode::getParameters() {
} else {
setAndGetNodeParameter(enable_stream_[stream_index], param_name, false);
}
param_name = "flip_" + stream_name_[stream_index];
setAndGetNodeParameter(flip_stream_[stream_index], param_name, false);
param_name = stream_name_[stream_index] + "_flip";
setAndGetNodeParameter<bool>(flip_stream_[stream_index], param_name, false);
param_name = stream_name_[stream_index] + "_mirror";
setAndGetNodeParameter<bool>(mirror_stream_[stream_index], param_name, false);
param_name = stream_name_[stream_index] + "_rotation";
setAndGetNodeParameter<int>(rotation_stream_[stream_index], param_name, -1);
param_name = camera_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);
orbbec_camera/src/ros_service.cpp
+92
View File
@@ -90,6 +90,20 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<SetBool::Response> response) {
setMirrorCallback(request, response, stream_index);
});
service_name = "set_" + stream_name + "_flip";
set_flip_srv_[stream_index] = node_->create_service<SetBool>(
service_name,
[this, stream_index = stream_index](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setFlipCallback(request, response, stream_index);
});
service_name = "set_" + stream_name + "_rotation";
set_rotation_srv_[stream_index] = node_->create_service<SetInt32>(
service_name,
[this, stream_index = stream_index](const std::shared_ptr<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
setRotationCallback(request, response, stream_index);
});
}
set_fan_work_mode_srv_ = node_->create_service<SetInt32>(
"set_fan_work_mode", [this](const std::shared_ptr<SetInt32::Request> request,
@@ -753,6 +767,84 @@ void OBCameraNode::setMirrorCallback(const std::shared_ptr<SetBool::Request>& re
}
}
void OBCameraNode::setFlipCallback(const std::shared_ptr<SetBool::Request>& request,
std::shared_ptr<SetBool::Response>& response,
const stream_index_pair& stream_index) {
(void)request;
auto stream = stream_index.first;
try {
switch (stream) {
case OB_STREAM_IR_RIGHT:
device_->setBoolProperty(OB_PROP_IR_RIGHT_FLIP_BOOL, request->data);
break;
case OB_STREAM_IR_LEFT:
device_->setBoolProperty(OB_PROP_IR_FLIP_BOOL, request->data);
break;
case OB_STREAM_IR:
device_->setBoolProperty(OB_PROP_IR_FLIP_BOOL, request->data);
break;
case OB_STREAM_DEPTH:
device_->setBoolProperty(OB_PROP_DEPTH_FLIP_BOOL, request->data);
break;
case OB_STREAM_COLOR:
device_->setBoolProperty(OB_PROP_COLOR_FLIP_BOOL, request->data);
break;
default:
RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
break;
}
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
response->success = false;
} catch (...) {
response->message = "unknown error";
response->success = false;
}
}
void OBCameraNode::setRotationCallback(const std::shared_ptr<SetInt32::Request>& request,
std::shared_ptr<SetInt32::Response>& response,
const stream_index_pair& stream_index) {
(void)request;
auto stream = stream_index.first;
try {
switch (stream) {
case OB_STREAM_IR_RIGHT:
device_->setIntProperty(OB_PROP_IR_RIGHT_ROTATE_INT, request->data);
break;
case OB_STREAM_IR_LEFT:
device_->setIntProperty(OB_PROP_IR_ROTATE_INT, request->data);
break;
case OB_STREAM_IR:
device_->setIntProperty(OB_PROP_IR_ROTATE_INT, request->data);
break;
case OB_STREAM_DEPTH:
device_->setIntProperty(OB_PROP_DEPTH_ROTATE_INT, request->data);
break;
case OB_STREAM_COLOR:
device_->setIntProperty(OB_PROP_COLOR_ROTATE_INT, request->data);
break;
default:
RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
break;
}
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
response->success = false;
} catch (const std::exception& e) {
response->message = e.what();
response->success = false;
} catch (...) {
response->message = "unknown error";
response->success = false;
}
}
void OBCameraNode::getLdpStatusCallback(const std::shared_ptr<GetBool::Request>& request,
std::shared_ptr<GetBool::Response>& response) {
(void)request;