diff --git a/include/iri_action_server/iri_action_server.h b/include/iri_action_server/iri_action_server.h index 1afc01c0af99c9a7115335f2e19b8e5320d632c6..112379322694329f313bf29b36b259bcf4f8bf75 100755 --- a/include/iri_action_server/iri_action_server.h +++ b/include/iri_action_server/iri_action_server.h @@ -185,6 +185,14 @@ class IriActionServer */ ros::Rate loop_rate_; + /** + * \brief Is Started + * + * This variable is set to true when start() is called and to false when + * calling shutdown(). + */ + bool is_started; + public: /** * \brief Constructor @@ -213,6 +221,20 @@ class IriActionServer */ void start(void); + /** + * \brief Server has been started? + * + * This function returns true if start() has been previously called. + */ + bool isStarted(void) const; + + /** + * \brief Stop Action + * + * Calls the ROS simple action server to shutdown the current action + */ + void shutdown(void); + /** * \brief Abort Action * @@ -299,7 +321,8 @@ template <class ActionSpec> IriActionServer<ActionSpec>::IriActionServer(ros::NodeHandle & nh, const std::string & aname) : action_name_(aname), as_(nh, action_name_, boost::bind(&IriActionServer<ActionSpec>::executeCallback, this, _1), false), - loop_rate_(10) + loop_rate_(10), + is_started(false) { ROS_DEBUG("IriActionServer::Constructor"); } @@ -335,11 +358,25 @@ void IriActionServer<ActionSpec>::start(void) { // launch action server as_.start(); + is_started = true; } else ROS_FATAL("Some Callbacks have not been registered yet!"); } +template <class ActionSpec> +bool IriActionServer<ActionSpec>::isStarted(void) const +{ + return is_started; +} + +template <class ActionSpec> +void IriActionServer<ActionSpec>::shutdown(void) +{ + as_.shutdown(); + is_started = false; +} + template <class ActionSpec> void IriActionServer<ActionSpec>::setAborted(const Result& result, const std::string& text) {