Add a basic action client

src/ros_lib/ros/action_client.h

@@ -0,0 +1,190 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2017, Robopec
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials provided
+ *    with the distribution.
+ *  * Neither the name of Willow Garage, Inc. nor the names of its
+ *    contributors may be used to endorse or promote prducts derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROS_ACTION_CLIENT_H_
+#define ROS_ACTION_CLIENT_H_
+
+#include <functional>
+#include <string>
+#include "rosserial_msgs/TopicInfo.h"
+
+#include "ros/publisher.h"
+#include "ros/subscriber.h"
+#include "../actionlib_msgs/GoalID.h"
+#include "../actionlib_msgs/GoalStatusArray.h"
+
+namespace ros {
+
+#define ACTION_DEFINITION(ActionSpec) \
+  typedef typename ActionSpec::_action_goal_type ActionGoal; \
+  typedef typename ActionGoal::_goal_type Goal; \
+  typedef typename ActionSpec::_action_result_type ActionResult; \
+  typedef typename ActionResult::_result_type Result; \
+  typedef typename ActionSpec::_action_feedback_type ActionFeedback; \
+  typedef typename ActionFeedback::_feedback_type Feedback;
+
+  template<typename ActionType>
+  class ActionClient {
+    ACTION_DEFINITION(ActionType)
+    public:
+      typedef std::function<void(const Feedback&)> FeedbackCallbackT;
+      typedef std::function<void(const Result&)> ResultCallbackT;
+      typedef std::function<void(uint8_t)> StateChangedCallbackT;
+
+      enum StateEnum
+      {
+        PENDING,
+        ACTIVE,
+        PREEMPTED,
+        SUCCEEDED,
+        ABORTED,
+        REJECTED,
+        PREEMPTING,
+        RECALLING,
+        RECALLED,
+        LOST,
+        INACTIVE
+      };
+
+      ActionClient(const std::string& action_name) :
+        goal_pub(action_name + "/goal"),
+        cancel_pub(action_name + "/cancel"),
+        status_sub(action_name + "/status", std::bind(&ActionClient::status_cb_internal, this, std::placeholders::_1)),
+        feedback_sub(action_name + "/feedback", std::bind(&ActionClient::feedback_cb_internal, this, std::placeholders::_1)),
+        result_sub(action_name + "/result", std::bind(&ActionClient::result_cb_internal, this, std::placeholders::_1))
+      {
+        resetState();
+      }
+
+      void sendGoal(const Goal& goal, FeedbackCallbackT feedback_cb = FeedbackCallbackT(), ResultCallbackT result_cb = ResultCallbackT())
+      {
+        if(!goal_pub.nh_->connected()) return;
+
+        feedback_cb_ = feedback_cb;
+        result_cb_ = result_cb;
+
+        ActionGoal action_goal_msg;
+        // no goal id and stamp -> will be generated by the action server
+        // see: http://wiki.ros.org/actionlib/DetailedDescription
+        action_goal_msg.goal = goal;
+        goal_pub.publish(action_goal_msg);
+      }
+
+      void cancelGoal()
+      {
+          actionlib_msgs::GoalID msg;
+          // no goal id and stamp -> cancel all goals
+          // see: http://wiki.ros.org/actionlib/DetailedDescription
+          cancel_pub.publish(msg);
+      }
+
+      void registerStateCallback(StateChangedCallbackT state_cb)
+      {
+        state_cb_ = state_cb;
+        resetState();
+      }
+
+      void unregisterStateCallback()
+      {
+          state_cb_ = StateChangedCallbackT();
+      }
+
+      uint8_t getState() const
+      {
+          return status_.status;
+      }
+
+      std::string getStateText() const
+      {
+          return status_.text;
+      }
+
+    private:
+      void status_cb_internal(const actionlib_msgs::GoalStatusArray& msg)
+      {
+        if(!msg.status_list.empty())
+        {
+          if(state_cb_ && (msg.status_list.back().status != status_.status))
+          {
+            state_cb_(msg.status_list.back().status);
+          }
+
+          status_ = msg.status_list.back();
+        }
+        else
+        {
+            resetState();
+        }
+      }
+
+      void feedback_cb_internal(const ActionFeedback& msg)
+      {
+        if(feedback_cb_)
+        {
+            feedback_cb_(msg.feedback);
+        }
+      }
+
+      void result_cb_internal(const ActionResult& msg)
+      {
+        if(result_cb_)
+        {
+            result_cb_(msg.result);
+        }
+      }
+
+      void resetState()
+      {
+        status_.goal_id = actionlib_msgs::GoalID();
+        status_.text = "inactive";
+        status_.status = INACTIVE;
+      }
+
+      FeedbackCallbackT feedback_cb_;
+      ResultCallbackT result_cb_;
+      StateChangedCallbackT state_cb_;
+      size_t goal_count_= 0;
+      actionlib_msgs::GoalStatus status_;
+
+    public:
+      Publisher<ActionGoal> goal_pub;
+      Publisher<actionlib_msgs::GoalID> cancel_pub;
+      Subscriber<actionlib_msgs::GoalStatusArray> status_sub;
+      Subscriber<ActionFeedback> feedback_sub;
+      Subscriber<ActionResult> result_sub;
+  };
+
+}
+
+#endif // ROS_ACTION_CLIENT_H_

src/ros_lib/ros/node_handle.h

@@ -62,6 +62,7 @@ class NodeHandleBase_ : public QObject{
 #include "ros/subscriber.h"
 #include "ros/service_server.h"
 #include "ros/service_client.h"
+#include "ros/action_client.h"
 
 namespace ros {
 
@@ -192,6 +193,17 @@ class NodeHandle : public NodeHandleBase_
     return v;
   }
 
+  /* Register a new Action Client */
+  template<typename ActionType>
+  bool actionClient(ActionClient<ActionType>& ac){
+    bool v = advertise(ac.goal_pub);
+    v |= advertise(ac.cancel_pub);
+    v |= subscribe(ac.status_sub);
+    v |= subscribe(ac.feedback_sub);
+    v |= subscribe(ac.result_sub);
+    return v;
+  }
+
   void negotiateTopics();
 
   virtual int publish(int id, const Msg * msg) override;