hubero_ros - created a thread for processing of the ROS callbacks [#57

]

- note: also got rid of `<functional>` from .cpp includes (locally only)

hubero_ros/include/hubero_ros/task_request_ros_api.h

@@ -21,6 +21,7 @@
 #include <hubero_ros_msgs/TeleopAction.h>
 
 #include <memory>
+#include <thread>
 
 namespace hubero {
 
@@ -33,12 +34,20 @@ namespace hubero {
  */
 class TaskRequestRosApi {
 public:
+	/// Defines the duration of sleeps between subsequent processings of the callback queue
+	const int CALLBACK_SPINNING_SLEEP_TIME_MS = 10;
+
 	/**
 	 * @brief Constructor of @ref TaskRequestRosApi instance
 	 * @param actor_name actor identifier from simulation
 	 */
 	TaskRequestRosApi(const std::string& actor_name);
 
+	/**
+	 * @brief Destructor that kills all spawned threads
+	 */
+	virtual ~TaskRequestRosApi();
+
 	/**
 	 * @defgroup tasks Methods that allow to request specific task from controlled actor
 	 * @{
@@ -63,8 +72,6 @@ class TaskRequestRosApi {
 
 	/**
 	 * @brief Returns most recent state of the task
-	 *
-	 * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used
 	 */
 	TaskFeedbackType getFollowObjectState() const;
 
@@ -108,8 +115,6 @@ class TaskRequestRosApi {
 	/**
 	 * @brief Returns most recent state of the task
 	 *
-	 * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used
-	 *
 	 * @note Currently: preempted -> finished
 	 */
 	TaskFeedbackType getLieDownState() const;
@@ -143,8 +148,6 @@ class TaskRequestRosApi {
 
 	/**
 	 * @brief Returns most recent state of the task
-	 *
-	 * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used
 	 */
 	TaskFeedbackType getMoveAroundState() const;
 
@@ -186,8 +189,6 @@ class TaskRequestRosApi {
 
 	/**
 	 * @brief Returns most recent state of the task
-	 *
-	 * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used
 	 */
 	TaskFeedbackType getMoveToGoalState() const;
 	TaskFeedbackType getMoveToObjectState() const;
@@ -220,8 +221,6 @@ class TaskRequestRosApi {
 
 	/**
 	 * @brief Returns most recent state of the task
-	 *
-	 * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used
 	 */
 	TaskFeedbackType getRunState() const;
 
@@ -264,8 +263,6 @@ class TaskRequestRosApi {
 
 	/**
 	 * @brief Returns most recent state of the task
-	 *
-	 * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used
 	 */
 	TaskFeedbackType getSitDownState() const;
 	TaskFeedbackType getSitDownObjectState() const;
@@ -294,8 +291,6 @@ class TaskRequestRosApi {
 
 	/**
 	 * @brief Returns most recent state of the task
-	 *
-	 * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used
 	 */
 	TaskFeedbackType getStandState() const;
 
@@ -338,8 +333,6 @@ class TaskRequestRosApi {
 
 	/**
 	 * @brief Returns most recent state of the task
-	 *
-	 * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used
 	 */
 	TaskFeedbackType getTalkState() const;
 	TaskFeedbackType getTalkObjectState() const;
@@ -372,8 +365,6 @@ class TaskRequestRosApi {
 
 	/**
 	 * @brief Returns most recent state of the task
-	 *
-	 * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used
 	 */
 	TaskFeedbackType getTeleopState() const;
 
@@ -498,9 +489,23 @@ class TaskRequestRosApi {
 		return action_client_ptr->getFeedbackText();
 	}
 
+	/**
+	 * A method that is executed in a separate thread (@ref callback_spinner_); periodically calls `ros::spinOnce`
+	 *
+	 * This is necessary for ActionServers to receive messages (otherwise is must've been handled by the user in his
+	 * orchestrating node).
+	 */
+	virtual void callbackProcessingThread();
+
 private:
 	/// Name of the actor provided in the constructor
 	std::string actor_name_;
 
+	/// A separate thread for processing callbacks
+	std::thread callback_spinner_;
+
+	/// Flag set in the destructor to join threads
+	std::atomic<bool> destructing_;
+
 }; // class TaskRequestRosApi
 } // namespace hubero

hubero_ros/src/task_request_ros_api.cpp

@@ -7,7 +7,9 @@
 namespace hubero {
 
 TaskRequestRosApi::TaskRequestRosApi(const std::string& actor_name):
-	node_ptr_(std::make_shared<Node>("task_request_ros_api_node"))
+	node_ptr_(std::make_shared<Node>("task_request_ros_api_node")),
+	callback_spinner_(std::thread(&TaskRequestRosApi::callbackProcessingThread, this)),
+	destructing_(false)
 {
 	// assignment - init list does not support string reference copying
 	actor_name_ = actor_name;
@@ -112,6 +114,13 @@ TaskRequestRosApi::TaskRequestRosApi(const std::string& actor_name):
 	);
 }
 
+TaskRequestRosApi::~TaskRequestRosApi() {
+	destructing_ = true;
+	if (callback_spinner_.joinable()) {
+		callback_spinner_.join();
+	}
+}
+
 bool TaskRequestRosApi::followObject(const std::string& object_name) {
 	hubero_ros_msgs::FollowObjectGoal action_goal;
 	action_goal.object_name = object_name;
@@ -405,4 +414,17 @@ std::string TaskRequestRosApi::getTeleopStateDescription() const {
 	return getActionStateDescription(ac_teleop_ptr_);
 }
 
+void TaskRequestRosApi::callbackProcessingThread() {
+	while (!destructing_) {
+		if (!ros::ok()) {
+			throw std::runtime_error(
+				"[HuBeRo] [TaskRequestRosApi].[callbackProcessingThread] ROS stopped working, "
+				"won't process any more callbacks"
+			);
+		}
+		ros::spinOnce();
+		std::this_thread::sleep_for(std::chrono::milliseconds(CALLBACK_SPINNING_SLEEP_TIME_MS));
+	}
+}
+
 } // namespace hubero