30 #include "ros_opcua_msgs/Address.h" 31 #include "ros_opcua_msgs/TypeValue.h" 33 #include "ros_opcua_srvs/CallMethod.h" 34 #include "ros_opcua_srvs/Connect.h" 35 #include "ros_opcua_srvs/Disconnect.h" 36 #include "ros_opcua_srvs/ListNode.h" 37 #include "ros_opcua_srvs/Read.h" 38 #include "ros_opcua_srvs/Subscribe.h" 39 #include "ros_opcua_srvs/Write.h" 40 #include "ros_opcua_srvs/Unsubscribe.h" 90 bool connect(ros_opcua_srvs::Connect::Request &req, ros_opcua_srvs::Connect::Response &res)
94 ROS_DEBUG(
"Establishing connection to OPC-UA server on address: %s", req.endpoint.c_str());
97 ROS_INFO(
"Connection to OPC-UA server on address '%s' established!", req.endpoint.c_str());
101 catch (
const std::exception& exc){
102 ROS_ERROR(
"OPC-UA client node %s: Connection to OPC-UA server on address %s failed! Message: %s",
ros::this_node::getName().c_str(), req.endpoint.c_str(), exc.what());
105 std::stringstream ss;
106 ss <<
"Connection to OPC-UA server on address " << req.endpoint <<
" failed! Message: " << exc.what();
107 res.error_message = ss.str();
110 ROS_ERROR(
"Connection to OPC-UA server on address %s failed with unknown exception", req.endpoint.c_str());
112 res.error_message =
"'Connect' service failed with Unknown exception";
123 bool disconnect(ros_opcua_srvs::Disconnect::Request &req, ros_opcua_srvs::Disconnect::Response &res)
125 ROS_DEBUG(
"Disconnecting from OPC-UA server...");
128 ROS_INFO(
"Disconnection succeded!");
132 catch (
const std::exception& exc){
133 ROS_ERROR(
"OPC-UA client node %s: Disconnection failed! (maybe client was not connected before?). Message: %s",
ros::this_node::getName().c_str(), exc.what());
136 res.error_message =
"Disconect service failed with exception: ";
137 res.error_message.append(exc.what());
140 ROS_ERROR(
"'Disconnect' service failed with unknown exception");
142 res.error_message =
"'Disconnect' service failed with unknown exception";
153 bool list_node(ros_opcua_srvs::ListNode::Request &req, ros_opcua_srvs::ListNode::Response &res)
159 if (req.node.nodeId ==
"") {
167 ros_opcua_msgs::Address address;
169 address.qualifiedName =
child.GetBrowseName().Name;
170 res.children.push_back(address);
174 catch (
const std::exception& exc){
175 ROS_ERROR(
"OPC-UA client node %s: 'ListNode' service called with node Id: %s, parameters failed! Exception: %s",
ros::this_node::getName().c_str(), req.node.nodeId.c_str(), exc.what());
178 res.error_message =
"ListNode service failed with exception: ";
179 res.error_message.append(exc.what());
183 ROS_ERROR(
"OPC-UA client node %s: 'ListNode' service called with node Id: %s parameters failed! Unknown exception!",
ros::this_node::getName().c_str(), req.node.nodeId.c_str());
186 res.error_message =
"ListNode service failed with Unknown exception";
191 bool call_method(ros_opcua_srvs::CallMethod::Request &req, ros_opcua_srvs::CallMethod::Response &res)
193 ROS_DEBUG(
"OPC-UA client node %s: 'CallMethod' service called with node Id: %s and %s parameters",
ros::this_node::getName().c_str(), req.node.nodeId.c_str(), req.method.nodeId.c_str());
198 std::vector<OpcUa::Variant> inputArguments;
200 for (ros_opcua_msgs::TypeValue typeValue : req.data) {
201 std::cout <<
"Variant: " <<
206 std::vector<OpcUa::Variant> outputArguments = node.
CallMethod(method_id, inputArguments);
215 catch (
const std::exception& exc){
216 ROS_ERROR(
"OPC-UA client node %s: 'CallMethod' service called with node Id: %s and %s parameters failed! Exception: %s",
ros::this_node::getName().c_str(), req.node.nodeId.c_str(), req.method.nodeId.c_str(), exc.what());
219 res.error_message =
"Call method service failed with exception: ";
220 res.error_message.append(exc.what());
224 ROS_ERROR(
"OPC-UA client node %s: 'CallMethod' service called with node Id: %s parameters failed! Unknown exception!",
ros::this_node::getName().c_str(), req.node.nodeId.c_str());
227 res.error_message =
"Call method service failed with Unknown exception";
238 bool read(ros_opcua_srvs::Read::Request &req, ros_opcua_srvs::Read::Response &res)
248 if (res.data.type ==
"Unknown") {
250 res.error_message =
"Unknown data type!!";
254 catch (
const std::exception& exc){
255 ROS_ERROR(
"OPC-UA client node %s: 'Read' service called with node Id: %s, parameters failed! Exception: %s",
ros::this_node::getName().c_str(), req.node.nodeId.c_str(), exc.what());
258 res.error_message =
"Read service failed with exception: ";
259 res.error_message.append(exc.what());
263 ROS_ERROR(
"OPC-UA client node %s: 'Read' service called with node Id: %s parameters failed! Unknown exception!",
ros::this_node::getName().c_str(), req.node.nodeId.c_str());
266 res.error_message =
"Read service failed with Unknown exception";
277 bool write(ros_opcua_srvs::Write::Request &req, ros_opcua_srvs::Write::Response &res)
286 if (!variant.
IsNul()) {
297 res.error_message =
"Unknown data type: ";
298 res.error_message.append(req.data.type);
302 catch (
const std::exception& exc){
303 ROS_ERROR(
"OPC-UA client node %s: 'Write' service called with node Id: %s, type: '%s' parameters failed! Exception: %s",
ros::this_node::getName().c_str(), req.node.nodeId.c_str(), req.data.type.c_str(), exc.what());
306 res.error_message =
"Write service failed with exception: ";
307 res.error_message.append(exc.what());
311 ROS_ERROR(
"OPC-UA client node %s: 'Write' service called with node Id: %s, type: '%s' parameters failed! Unknown exception!",
ros::this_node::getName().c_str(), req.node.nodeId.c_str(), req.data.type.c_str());
314 res.error_message =
"Write service failed with Unknown exception";
327 bool subscribe(ros_opcua_srvs::Subscribe::Request &req, ros_opcua_srvs::Subscribe::Response &res)
338 _callback_publishers[node_string] = nodeHandle.advertise<ros_opcua_msgs::TypeValue>(req.callback_topic, 1);
343 ROS_INFO(
"Node successfully subscribed!");
346 catch (
const std::exception& exc){
347 ROS_ERROR(
"OPC-UA client node %s: 'Subscribe' service called with node Id: %s parameters failed!",
ros::this_node::getName().c_str(), req.node.nodeId.c_str());
350 res.error_message =
"Subscribe service failed with exception: ";
351 res.error_message.append(exc.what());
355 ROS_ERROR(
"OPC-UA client node %s: 'Subscribe' service called with node Id: %s parameters failed!",
ros::this_node::getName().c_str(), req.node.nodeId.c_str());
358 res.error_message =
"Subscribe service failed with Unknown exception";
370 bool unsubscribe(ros_opcua_srvs::Unsubscribe::Request &req, ros_opcua_srvs::Unsubscribe::Response &res)
389 ROS_INFO(
"Node successfully unsubscribed!");
392 catch (
const std::exception& exc){
393 ROS_ERROR(
"OPC-UA client node %s: 'Unsubscribe' service called with node Id: %s parameters failed!",
ros::this_node::getName().c_str(), req.node.nodeId.c_str());
396 res.error_message =
"Unsubscribe service failed with exception: ";
397 res.error_message.append(exc.what());
401 ROS_ERROR(
"OPC-UA client node %s: 'Unsubscribe' service called with node Id: %s parameters failed!",
ros::this_node::getName().c_str(), req.node.nodeId.c_str());
404 res.error_message =
"Unsubscribe service failed with Unknown exception";
426 int main (
int argc,
char** argv)
428 ros::init(argc, argv,
"opcua_client_node");
SubClient _sclt
Subscription client store.
Node GetNode(const NodeId &nodeid) const
Get a specific node by nodeid.
bool disconnect(ros_opcua_srvs::Disconnect::Request &req, ros_opcua_srvs::Disconnect::Response &res)
bool subscribe(ros_opcua_srvs::Subscribe::Request &req, ros_opcua_srvs::Subscribe::Response &res)
int main(int argc, char **argv)
bool connect(ros_opcua_srvs::Connect::Request &req, ros_opcua_srvs::Connect::Response &res)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Subscription client class handles subscription callbacks.
ROSCPP_DECL const std::string & getName()
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void Disconnect()
Disconnect from server.
bool write(ros_opcua_srvs::Write::Request &req, ros_opcua_srvs::Write::Response &res)
bool unsubscribe(ros_opcua_srvs::Unsubscribe::Request &req, ros_opcua_srvs::Unsubscribe::Response &res)
std::map< std::string, ros::Publisher > _callback_publishers
Callback publishers list.
void Connect(const std::string &endpoint)
connect to a server, specify endpoint as string
bool list_node(ros_opcua_srvs::ListNode::Request &req, ros_opcua_srvs::ListNode::Response &res)
std::string getService() const
bool read(ros_opcua_srvs::Read::Request &req, ros_opcua_srvs::Read::Response &res)
Node GetObjectsNode() const
std::vector< Variant > CallMethod(NodeId methodId, std::vector< Variant > inputArguments) const
Subscription::SharedPtr CreateSubscription(unsigned int period, SubscriptionHandler &client)
Create a subscription objects.
A Node object represent an OPC-UA node. It is high level object intended for developper who want to e...
NodeId ToNodeId(const std::string &str, uint32_t defaultNamespace=0)
void DataChange(uint32_t handle, const OpcUa::Node &node, const OpcUa::Variant &value, OpcUa::AttributeId attr) override
ros_opcua_msgs::TypeValue convertVariantToTypeValue(const OpcUa::Variant &variant)
std::string ToString(const AttributeId &value)
bool _connected
Connected status variable.
void SetValue(const Variant &val) const
ROSCPP_DECL void shutdown()
std::map< std::string, uint32_t > _subscription_handles
List of subscription handles.
bool call_method(ros_opcua_srvs::CallMethod::Request &req, ros_opcua_srvs::CallMethod::Response &res)
OpcUa::Variant convertTypeValueToVariant(ros_opcua_msgs::TypeValue &typeValue)
std::map< std::string, std::shared_ptr< OpcUa::Subscription > > _subscriptions
Subscription list.
OpcUa::UaClient _client(false)
Client variable.
This file contains helper functions for ROS implementation of OpcUa client.
void on_shutdown(int sig)
std::string ToString() const