diff --git a/include/iri_action_server/iri_action_server.h b/include/iri_action_server/iri_action_server.h index 65cb665aa6c69690cc6b567c542675389bff194c..55f218f68614fe23b519f5e6216566d5d01b2686 100755 --- a/include/iri_action_server/iri_action_server.h +++ b/include/iri_action_server/iri_action_server.h @@ -42,13 +42,15 @@ class IriActionServer public: //generates typedefs that we'll use to make our lives easier ACTION_DEFINITION(ActionSpec); + typedef boost::shared_ptr<Result> ResultPtr; + typedef boost::shared_ptr<Feedback> FeedbackPtr; private: boost::function<void (const GoalConstPtr&)> start_action_callback_; boost::function<bool ()> is_finished_callback_; boost::function<bool ()> has_succeed_callback_; - boost::function<void (ResultConstPtr&)> get_result_callback_; - boost::function<void (FeedbackConstPtr&)> publish_feedback_callback_; + boost::function<void (ResultPtr&)> get_result_callback_; + boost::function<void (FeedbackPtr&)> publish_feedback_callback_; void executeCallback(const GoalConstPtr& goal); @@ -75,7 +77,7 @@ class IriActionServer * * \param nh a reference to the node handle object to manage all ROS topics. */ - IriActionServer(const std::string & action_name, ros::NodeHandle & nh); + IriActionServer(ros::NodeHandle & nh, const std::string & action_name); /** * \brief Destructor @@ -90,17 +92,19 @@ class IriActionServer void registerStartCallback(boost::function<void (const GoalConstPtr&)> cb); void registerIsFinishedCallback( boost::function<bool ()> cb); void registerHasSucceedCallback( boost::function<bool ()> cb); - void registerGetResultCallback(boost::function<void (ResultConstPtr&)> cb); - void registerPublishFeedbackCallback(boost::function<void (FeedbackConstPtr&)> cb); + void registerGetResultCallback(boost::function<void (ResultPtr&)> cb); + void registerPublishFeedbackCallback(boost::function<void (FeedbackPtr&)> cb); }; template <class ActionSpec> -IriActionServer<ActionSpec>::IriActionServer(const std::string & aname, ros::NodeHandle & nh) : +IriActionServer<ActionSpec>::IriActionServer(ros::NodeHandle & nh, const std::string & aname) : action_name_(aname), - as_(nh, action_name_, boost::bind(&IriActionServer<ActionSpec>::executeCallback, this, _1), true), + as_(nh, action_name_, boost::bind(&IriActionServer<ActionSpec>::executeCallback, this, _1), false), loop_rate_(10) { + // launch action server + as_.start(); } template <class ActionSpec> @@ -149,7 +153,7 @@ void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal) is_active = false; // get action result - ResultConstPtr result; + ResultPtr result; get_result_callback_(result); // action has been successfully accomplished @@ -169,7 +173,7 @@ void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal) else { // get action feedback and publish it - FeedbackConstPtr feedback; + FeedbackPtr feedback; publish_feedback_callback_(feedback); as_.publishFeedback(*feedback); } @@ -198,13 +202,13 @@ void IriActionServer<ActionSpec>::registerHasSucceedCallback( boost::function<bo } template <class ActionSpec> -void IriActionServer<ActionSpec>::registerGetResultCallback(boost::function<void (ResultConstPtr&)> cb) +void IriActionServer<ActionSpec>::registerGetResultCallback(boost::function<void (ResultPtr&)> cb) { get_result_callback_ = cb; } template <class ActionSpec> -void IriActionServer<ActionSpec>::registerPublishFeedbackCallback(boost::function<void (FeedbackConstPtr&)> cb) +void IriActionServer<ActionSpec>::registerPublishFeedbackCallback(boost::function<void (FeedbackPtr&)> cb) { publish_feedback_callback_ = cb; }