Merge pull request #5 from 1r0b1n0/service_result

Add service client call result
1r0b1n0 · Nov 7, 2018 · d70c396 · d70c396
2 parents d906b92 + 5a25212
commit d70c396
Show file tree

Hide file tree

Showing 3 changed files with 60 additions and 345 deletions.
diff --git a/src/ros_lib/ros/service_client.h b/src/ros_lib/ros/service_client.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * Software License Agreement (BSD License)
  *
  * Copyright (c) 2011, Willow Garage, Inc.
@@ -36,6 +36,7 @@
 #define ROS_SERVICE_CLIENT_H_
 
 #include <functional>
+#include "rosserial_msgs/ServiceCallResult.h"
 #include "rosserial_msgs/TopicInfo.h"
 
 #include "ros/publisher.h"
@@ -50,48 +51,67 @@ namespace ros {
       typedef typename MType::Request MReq;
       typedef typename MType::Response MRes;
       typedef std::function<void(const MRes&, bool)> CallbackT;
+      typedef std::function<void(const MRes&, bool, uint8_t)> CallbackWithResultT;
 
-      ServiceClient(const char* topic_name) : 
+      ServiceClient(const char* topic_name) :
         pub(topic_name, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER)
       {
         this->topic_ = topic_name;
         this->waiting = false;
       }
 
-      void call(const MReq & request, CallbackT callback)
+      void call(const MReq& request, CallbackT callback)
       {
         if(!pub.nh_->connected()) return;
         waiting = true;
         cb_ = callback;
         pub.publish(request);
       }
 
-      const std::string & getMsgType() override { return MRes::getType(); }
-      const std::string & getMsgMD5() override { return MRes::getMD5(); }
+      void call(const MReq& request, CallbackWithResultT callback)
+      {
+        if(!pub.nh_->connected()) return;
+        waiting = true;
+        cbWithResult_ = callback;
+        pub.publish(request);
+      }
+
+      const std::string& getMsgType() override { return MRes::getType(); }
+      const std::string& getMsgMD5() override { return MRes::getMD5(); }
       int getEndpointType() override{ return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; }
 
     private:
       // these refer to the subscriber
       void callback(unsigned char *data) override{
         MRes ret;
         // First byte give us the success status
-        bool success = data[0];
+        const uint8_t callResult = data[0];
+        const bool success = callResult == rosserial_msgs::ServiceCallResult::SUCCESS;
 
         if(success)
         {
             // Access from the second byte
             ret.deserialize(data+1);
         }
+
         waiting = false;
+
         if(cb_)
         {
           cb_(ret, success);
         }
+        else if(cbWithResult_)
+        {
+          cbWithResult_(ret, success, callResult);
+        }
+
         cb_ = nullptr;
+        cbWithResult_ = nullptr;
       }
 
       bool waiting;
       CallbackT cb_;
+      CallbackWithResultT cbWithResult_;
 
     public:
       Publisher<MReq> pub;

diff --git a/test/Basic/node.cpp b/test/Basic/node.cpp
@@ -1,5 +1,6 @@
 #include "node.h"
 #include <roscpp_tutorials/TwoInts.h>
+#include <rosserial_msgs/ServiceCallResult.h>
 
 Node::Node():
   chatter("chatter"),
@@ -43,10 +44,40 @@ void Node::onTimer()
   chatter.publish(str_msg);
 
   roscpp::GetLoggersRequest req;
-  serviceClient.call(req, [](const roscpp::GetLoggers::Response &loggers){
-    std::cout << "loggers : " << QJsonDocument(loggers.serializeAsJson()).toJson(QJsonDocument::Indented).toStdString() << std::endl;
 
-  });
+  if(i % 2 == 0)
+  {
+    std::cout << "Service client call with bool and uint8_t results API" << std::endl;
+    serviceClient.call(req, [this](const roscpp::GetLoggers::Response &loggers, bool ok, uint8_t callResult){
+      (void)ok;
+      switch(callResult)
+      {
+      case rosserial_msgs::ServiceCallResult::NO_EXISTENCE:
+          std::cout << "Service " << serviceClient.topic_ << " does not exist" << std::endl;
+          break;
+      case rosserial_msgs::ServiceCallResult::CALL_FAILED:
+          std::cout << "Call to service " << serviceClient.topic_ << " failed" << std::endl;
+          break;
+      case rosserial_msgs::ServiceCallResult::SUCCESS:
+        std::cout << "loggers : " << QJsonDocument(loggers.serializeAsJson()).toJson(QJsonDocument::Indented).toStdString() << std::endl;
+        break;
+      }
+    });
+  }
+  else
+  {
+    std::cout << "Service client call with only bool result API" << std::endl;
+    serviceClient.call(req, [this](const roscpp::GetLoggers::Response &loggers, bool ok){
+      if(ok)
+      {
+          std::cout << "loggers : " << QJsonDocument(loggers.serializeAsJson()).toJson(QJsonDocument::Indented).toStdString() << std::endl;
+      }
+      else
+      {
+          std::cout << "Call to service " << serviceClient.topic_ << " failed" << std::endl;
+      }
+    });
+  }
 }
 
 void Node::addTwoInts(const roscpp_tutorials::TwoIntsRequest &req, roscpp_tutorials::TwoIntsResponse &res)