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

add get ldp status

Browse Source
This commit is contained in:
Joe Dong
2023-02-17 15:42:53 +08:00
parent ba696d46c5
commit 2a37e884c1
3 changed files with 85 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+14 -3
View File
@@ -44,6 +44,7 @@
#include "orbbec_camera_msgs/srv/get_int32.hpp"
#include "orbbec_camera_msgs/srv/get_string.hpp"
#include "orbbec_camera_msgs/srv/set_int32.hpp"
#include "orbbec_camera_msgs/srv/get_bool.hpp"
#include "orbbec_camera/constants.h"
#include "orbbec_camera/dynamic_params.h"
@@ -80,6 +81,7 @@ using SetInt32 = orbbec_camera_msgs::srv::SetInt32;
using GetInt32 = orbbec_camera_msgs::srv::GetInt32;
using GetString = orbbec_camera_msgs::srv::GetString;
using SetBool = std_srvs::srv::SetBool;
using GetBool = orbbec_camera_msgs::srv::GetBool;
typedef std::pair<ob_stream_type, int> stream_index_pair;
@@ -191,8 +193,8 @@ class OBCameraNode {
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
void setFanModeCallback(const std::shared_ptr<SetInt32::Request>& request,
std::shared_ptr<SetInt32::Response>& response);
void setFanWorkModeCallback(const std::shared_ptr<SetInt32::Request>& request,
std::shared_ptr<SetInt32::Response>& response);
void getDeviceInfoCallback(const std::shared_ptr<GetDeviceInfo::Request>& request,
std::shared_ptr<GetDeviceInfo::Response>& response);
@@ -204,6 +206,13 @@ class OBCameraNode {
std::shared_ptr<SetBool::Response>& response,
const stream_index_pair& stream_index);
void setMirrorCallback(const std::shared_ptr<SetBool::Request>& request,
std::shared_ptr<SetBool::Response>& response,
const stream_index_pair& stream_index);
void getLdpStatusCallback(const std::shared_ptr<GetBool::Request>& request,
std::shared_ptr<GetBool::Response>& response);
bool toggleSensor(const stream_index_pair& stream_index, bool enabled, std::string& msg);
void publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
@@ -266,6 +275,7 @@ class OBCameraNode {
std::map<stream_index_pair, rclcpp::Service<GetInt32>::SharedPtr> get_gain_srv_;
std::map<stream_index_pair, rclcpp::Service<SetInt32>::SharedPtr> set_gain_srv_;
std::map<stream_index_pair, rclcpp::Service<SetBool>::SharedPtr> toggle_sensor_srv_;
std::map<stream_index_pair, rclcpp::Service<SetBool>::SharedPtr> set_mirror_srv_;
rclcpp::Service<GetInt32>::SharedPtr get_white_balance_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_white_balance_srv_;
rclcpp::Service<GetInt32>::SharedPtr get_auto_white_balance_srv_;
@@ -276,8 +286,9 @@ class OBCameraNode {
rclcpp::Service<GetDeviceInfo>::SharedPtr get_device_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_laser_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ldp_enable_srv_;
rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_ldp_status_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_floor_enable_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_fan_mode_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_fan_work_mode_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
bool publish_tf_ = false;
orbbec_camera/src/ros_service.cpp
+70 -5
View File
@@ -71,11 +71,18 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<SetBool::Response> response) {
toggleSensorCallback(request, response, stream_index);
});
service_name = "set_" + stream_name + "_mirror";
set_mirror_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) {
setMirrorCallback(request, response, stream_index);
});
}
set_fan_mode_srv_ = node_->create_service<SetInt32>(
set_fan_work_mode_srv_ = node_->create_service<SetInt32>(
"set_fan_work_mode", [this](const std::shared_ptr<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
setFanModeCallback(request, response);
std::shared_ptr<SetInt32::Response> response) {
setFanWorkModeCallback(request, response);
});
set_floor_enable_srv_ = node_->create_service<SetBool>(
"set_floor_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
@@ -95,6 +102,13 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<SetBool::Response> response) {
setLdpEnableCallback(request_header, request, response);
});
get_ldp_status_srv_ = node_->create_service<GetBool>(
"get_ldp_status", [this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<GetBool::Request> request,
std::shared_ptr<GetBool::Response> response) {
(void)request_header;
getLdpStatusCallback(request, response);
});
get_white_balance_srv_ = node_->create_service<GetInt32>(
"get_white_balance", [this](const std::shared_ptr<GetInt32::Request> request,
@@ -329,8 +343,8 @@ void OBCameraNode::setAutoExposureCallback(
}
}
void OBCameraNode::setFanModeCallback(const std::shared_ptr<SetInt32::Request>& request,
std::shared_ptr<SetInt32::Response>& response) {
void OBCameraNode::setFanWorkModeCallback(const std::shared_ptr<SetInt32::Request>& request,
std::shared_ptr<SetInt32::Response>& response) {
(void)response;
bool fan_mode = request->data;
try {
@@ -495,6 +509,57 @@ void OBCameraNode::getSDKVersion(const std::shared_ptr<GetString::Request>& requ
}
}
void OBCameraNode::setMirrorCallback(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:
device_->setBoolProperty(OB_PROP_IR_MIRROR_BOOL, request->data);
break;
case OB_STREAM_DEPTH:
device_->setBoolProperty(OB_PROP_DEPTH_MIRROR_BOOL, request->data);
break;
case OB_STREAM_COLOR:
device_->setBoolProperty(OB_PROP_COLOR_MIRROR_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::getLdpStatusCallback(const std::shared_ptr<GetBool::Request>& request,
std::shared_ptr<GetBool::Response>& response) {
(void)request;
try {
response->data = device_->getBoolProperty(OB_PROP_LDP_STATUS_BOOL);
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::toggleSensorCallback(const std::shared_ptr<SetBool::Request>& request,
std::shared_ptr<SetBool::Response>& response,
const stream_index_pair& stream_index) {