diff --git a/hubero_ros/include/hubero_ros/task_request_ros_api.h b/hubero_ros/include/hubero_ros/task_request_ros_api.h index ef340ee..520377c 100644 --- a/hubero_ros/include/hubero_ros/task_request_ros_api.h +++ b/hubero_ros/include/hubero_ros/task_request_ros_api.h @@ -21,6 +21,7 @@ #include #include +#include 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 destructing_; + }; // class TaskRequestRosApi } // namespace hubero diff --git a/hubero_ros/src/task_request_ros_api.cpp b/hubero_ros/src/task_request_ros_api.cpp index 121eb68..939801d 100644 --- a/hubero_ros/src/task_request_ros_api.cpp +++ b/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("task_request_ros_api_node")) + node_ptr_(std::make_shared("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