mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-05 20:47:46 +08:00
79 lines
3.0 KiB
C++
79 lines
3.0 KiB
C++
/*******************************************************************************
|
|
* Copyright (c) 2026 Orbbec 3D Technology, Inc
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*******************************************************************************/
|
|
|
|
#include "orbbec_camera/gige_action_command_node.h"
|
|
|
|
#include <exception>
|
|
#include <functional>
|
|
#include <sstream>
|
|
#include <string>
|
|
|
|
#include "rclcpp_components/register_node_macro.hpp"
|
|
|
|
namespace orbbec_camera {
|
|
namespace {
|
|
|
|
std::string formatObError(const ob::Error& error) {
|
|
std::ostringstream stream;
|
|
stream << (error.getMessage() ? error.getMessage() : "Unknown OB error")
|
|
<< " status:" << static_cast<int>(error.getStatus());
|
|
return stream.str();
|
|
}
|
|
|
|
} // namespace
|
|
|
|
GigEActionCommandNode::GigEActionCommandNode(const rclcpp::NodeOptions& node_options)
|
|
: Node("gige_action_command_node", node_options), context_(std::make_unique<ob::Context>()) {
|
|
context_->enableNetDeviceEnumeration(true);
|
|
send_action_command_service_ = create_service<orbbec_camera_msgs::srv::SendActionCommand>(
|
|
"~/send_action_command", std::bind(&GigEActionCommandNode::sendActionCommandCallback, this,
|
|
std::placeholders::_1, std::placeholders::_2));
|
|
RCLCPP_INFO(get_logger(), "GigE Action Command service is ready");
|
|
}
|
|
|
|
void GigEActionCommandNode::sendActionCommandCallback(
|
|
const std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Request> request,
|
|
std::shared_ptr<orbbec_camera_msgs::srv::SendActionCommand::Response> response) {
|
|
if (!request) {
|
|
response->success = false;
|
|
response->message = "Invalid request";
|
|
return;
|
|
}
|
|
|
|
const std::string destination_ip =
|
|
request->destination_ip.empty() ? "255.255.255.255" : request->destination_ip;
|
|
try {
|
|
response->success =
|
|
context_->sendActionCommand(request->device_key, request->group_key, request->group_mask,
|
|
destination_ip.c_str(), request->scheduled_time);
|
|
response->message =
|
|
response->success ? "Action Command dispatched" : "SDK failed to send Action Command";
|
|
} catch (const ob::Error& error) {
|
|
response->success = false;
|
|
response->message = formatObError(error);
|
|
} catch (const std::exception& error) {
|
|
response->success = false;
|
|
response->message = error.what();
|
|
} catch (...) {
|
|
response->success = false;
|
|
response->message = "Unknown error";
|
|
}
|
|
}
|
|
|
|
} // namespace orbbec_camera
|
|
|
|
RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::GigEActionCommandNode)
|