module_action.h 25.7 KB
Newer Older
1
2
3
4
#ifndef _MODULE_ACTION_H
#define _MODULE_ACTION_H

#include <iri_ros_tools/module_common.h>
5
#include <iri_ros_tools/module_exceptions.h>
6
7
8
9
10
11
12
13
#include <ros/ros.h>

#include <iri_ros_tools/timeout.h>
#include <iri_ros_tools/watchdog.h>

#include <actionlib/client/simple_action_client.h>
#include <actionlib/action_definition.h>

14
#include "mutex.h"
15
16
17
18
19

#define     DEFAULT_ACTION_MAX_RETRIES     5
#define     DEFAULT_ACTION_TIMEOUT         10
#define     DEFAULT_WATCHDOG_TIME          2

20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
/**
  * \brief Possible action states returned by the get_state() function
  * 
  * These are the possible states of the action associated to this class. Most
  * of them represent actual states of the goal inside the action server, except
  * for two:
  *  * ACTION_TIMEOUT: indicates the action did not finish in the alloed time
  *  * ACTION_WATCHDOG: indicate the action has not received feedback for a long 
  *                     time
  */
typedef enum {ACTION_IDLE,
              ACTION_RUNNING,
              ACTION_SUCCESS,
              ACTION_TIMEOUT,
              ACTION_FB_WATCHDOG,
              ACTION_ABORTED,
36
37
              ACTION_PREEMPTED,
              ACTION_REJECTED} action_status;
38

39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
/**
  * \brief Action client wrapper
  *
  * This class wraps a simple ROS action client in order to simplify its use.
  * This class is a template of any ROS action and provides the common features
  * to start, cancel and monitor the current state of the goal. In addition, it
  * provides the following features:
  *
  *  * the maximum number of times the action can fail to start before reporting 
  *    an error.
  *  * Timeout to control the maximum time the action can be active.
  *  * A watchdog to monitor the correct reception of the feedback of the action.
  * 
  * All these new features can be configured using the class API. 
  *
  * The action topics are created inside the namespace defined by the (optional)
  * namespace and the actual name provided at contruction time as follows
  * 
  *   <main node namespace>/<class namespace>/<class name>
  */
59
60
61
62
template<class action_ros>
class CModuleAction
{
  private:
63
64
65
66
67
68
    /**
      * \brief Internal Mutex
      *
      * This Mutex is used to ensure atomic access to the internal attributes 
      * of the class.
      */
69
    CMutex action_access;
70
71
72
73
74
75
76
    /**
      * \brief name of the action object
      *
      * Attribute with the name of the action object, together with the full 
      * namespace. This name is used to report information, errors and 
      * warnings regarding the action object.
      */
77
    std::string name;
78
79
80
81
82
83
84
    /**
      * \brief Status of the action
      * 
      * This attribute holds the current action of the object. This can be any
      * value of the action_status data type. This status is updated internally
      * and the get_state() function should be used to access it.
      */
85
86
    action_status status;
    // number of retries attributes
87
88
89
90
91
92
    /**
      * \brief Number of tries to start the action
      * 
      * This attribute holds the number of attempts to start the action. It is 
      * only used internally to handle possible start failures.
      */
93
    unsigned int current_num_retries;
94
95
96
97
98
99
100
101
    /**
      * \brief maximum number of start attempts
      * 
      * This attribute holds the maximum number of attempts to start the action.
      * This value can be modified by the user with the set_max_num_retries() 
      * functions, and the actual value can be obtain with the get_max_num_retries()
      * functions.
      */
102
103
    unsigned int max_num_retries;
    // ROS node handle
104
105
106
107
108
109
110
111
112
    /**
      * \brief internal ROS node handle
      *
      * A ROS node handle used to initialize the internal ROS action. This node 
      * handle is initialized at construction time as follows:
      * 
      * /<node namespace>/<provided namespace>/<provided name>
      *
      */
113
114
    ros::NodeHandle nh;
    // timeout attributes
115
116
117
118
    /**
      * \brief Use the timeout feature
      * 
      * This attribute specifies whether the timeout feature is to be used to limit
119
      * the amount of time the action is active or not. Its value is controlled by
120
121
122
123
124
      * the enable_timeout() and disable_timeout() functions.
      *
      * To check whether the timeout is active or not, use the is_timeout_enabled()
      * function.
      */
125
    bool use_timeout;
126
127
128
129
130
131
132
    /**
      * \brief Timeout value in seconds
      * 
      * This is the actual maximum time allowed to finish the action before reporting 
      * and error. This value can be set when starting a timeout with the start_timeout()
      * function or when updating the value with the update_timeout() function.
      */
133
    double timeout_value;
134
135
136
137
138
139
    /**
      * \brief Timeout object
      * 
      * This is a timeout object actually used to handle the timeout feature of this 
      * class. See the documentation of this class for further details.
      */
140
141
    CROSTimeout action_timeout;
    // watchdog attributes
142
143
144
145
146
147
148
149
150
151
152
    /**
      * \brief Use the watchdog feature
      * 
      * This attribute specifies whether the watchdog feature is to be used to limit
      * the amount of time between action feedback messages. Its value is controlled by
      * the enable_watchdog() and disable_watchdog() functions.
      *
      * To check whether the watchdog is active or not, use the is_watchdog_enabled()
      * function.
      */
    bool use_watchdog;
153
154
155
156
157
158
159
    /**
      * \brief Maximum time between feedback messages
      * 
      * This attribute holds the maximum time in seconds allowed between two consecutive
      * feedback messages. This value can be modified with the set_feedback_watchdog_time().
      * To get the current value use the get_feedback_watchdog_time() function.
      */
160
    double watchdog_time;
161
162
163
164
165
166
167
    /**
      * \brief Watchdog object
      * 
      * This is the watchdog object actually used to handle the watchdog feature of this
      * class. This feature is also anables. See the documentation of this class for
      * further details.
      */
168
    CROSWatchdog feedback_watchdog;
169
170
171
172
173
    /**
      * \brief
      *
      */
    bool enabled;
174
    // action attributes
175
176
177
178
179
    /**
      * \brief ROS topic messages names
      * 
      * This declaration defines all the ROS topis names of the provided action.
      */
180
    ACTION_DEFINITION(action_ros);
181
182
183
184
185
186
    /**
      * \brief Action client object
      * 
      * This points to the ROS action client object. See the documentation on this
      * class for further details.
      */
187
    actionlib::SimpleActionClient<action_ros> *action_client;
188
189
190
191
192
193
194
    /**
      * \brief Result message of the action
      * 
      * This attribute holds the result message of the last executed action. Use 
      * the get_result() function to get it. This data structure is updated by
      * the action_done() callback.
      */
195
    Result action_result_msg;
196
197
198
199
200
201
202
    /**
      * \brief Feedback message of the action
      * 
      * This attribute holds the last feedback message received of the current 
      * action. Use the get_feedback() function to get it. This data structure
      * is updated by the action_feedback() callback.
      */
203
    Feedback action_feedback_msg;
204
205
206
207
208
209
210
211
212
213
214
215
216
    /**
      * \brief Action done callback
      * 
      * This callback is called whenever the current action finishes. It stores 
      * the user defined result data structure into the internal attribute and
      * reports how the action ended, both using the rosout feature and the
      * internal status attribute.
      *
      * \param state termination state of the current action. Defined by ROS.
      *
      * \param result action dependant result data structure received from the
      *               action server.
      */
217
218
219
220
221
222
223
224
    void action_done(const actionlib::SimpleClientGoalState& state,const ResultConstPtr& result)
    {
      this->action_access.enter();
      if(state==actionlib::SimpleClientGoalState::ABORTED)
      {
        ROS_ERROR_STREAM("CModuleAction::action_done: goal on server " << this->name << " aborted");
        this->status=ACTION_ABORTED;
      }
225
226
227
228
229
      if(state==actionlib::SimpleClientGoalState::REJECTED)
      {
        ROS_ERROR_STREAM("CModuleAction::action_done: goal on server " << this->name << " rejected");
        this->status=ACTION_REJECTED;
      }
230
231
232
233
234
235
236
      else if(state==actionlib::SimpleClientGoalState::PREEMPTED)
      {
        ROS_WARN_STREAM("CModuleAction::action_done: goal on server " << this->name << " preempted");
        this->status=ACTION_PREEMPTED;
      }
      else if(state==actionlib::SimpleClientGoalState::SUCCEEDED)
      {
237
        ROS_INFO_STREAM("CModuleAction::action_done: goal on server " << this->name << " successfull");
238
239
240
        this->status=ACTION_SUCCESS;
      } 
      this->action_result_msg=*result;
241
      this->action_feedback_msg=Feedback();
242
243
      this->action_access.exit();
    }
244
245
246
247
248
249
    /**
      * \brief Action active callback
      * 
      * This callback is called when the action becomes active, after it has been 
      * received by the server.
      */
250
    void action_active(void);
251
252
253
254
255
256
257
258
259
    /**
      * \brief Action feedback callback
      * 
      * This callback is called whenever new feedback data is received from the 
      * action server. This function updated the internal feedback data structure.
      *
      * \param feedback action dependant feedback data structure received from the
      *                 action server.
      */
260
261
262
263
264
    void action_feedback(const FeedbackConstPtr& feedback)
    {
      ROS_DEBUG_STREAM("CModuleAction::action_feedback: Got Feedback from server " << this->name << "!");
      this->action_access.enter();
      this->action_feedback_msg=*feedback;
265
266
      if(this->use_watchdog)
        this->feedback_watchdog.reset(ros::Duration(this->watchdog_time));
267
268
269
      this->action_access.exit();
    }
  public:
270
271
272
273
274
275
276
277
278
279
280
    /**
      * \brief Constructor
      * 
      * Constructor of the action class. This function initializes and configures all
      * the internal attributes, including the action client. 
      *   
      * \param name string with the desired name for the action.
      *
      * \param namespace string with the base namespace for the action.

      */
281
    CModuleAction(const std::string &name,const std::string &name_space=std::string(""));
282
283
284
285
286
287
288
289
290
    /**
      * \brief Sets the maximum number of attempts to start the action
      * 
      * This function sets the maximum number of attempts allowed to start the
      * action before giving up. By default this value is set to 5.
      *
      * \param max_retries non-negative integer with the maximum number of attempts
      *                    to start the action
      */
291
    void set_max_num_retries(unsigned int max_retries);
292
293
294
295
296
297
298
299
300
    /**
      * \brief Gets the maximum number of attempts to start the action
      * 
      * This function returns the  maximum number of attempts allowed to start the
      * action before giving up. By default this value is set to 5.
      * 
      * \return non-negative integer with the maximum number of attempts to start 
      *         the action
      */
301
    unsigned int get_max_num_retries(void);
302
    /**
303
      * \brief Enables the actiin feedback watchdog
304
      * 
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
      * This function enables the internal action feedback watchdog to limit 
      * the maximum time between feedback messages. By default this feature is 
      * enabled.
      *
      * \param time_s the desired maximum time between feedback messages before 
      *               reporting and error.
      */
    void enable_watchdog(double time_s);
    /**
      * \brief Disables the action feedback watchdog
      * 
      * This function disables the internal action feedback watchdog to limit 
      * the maximum time between feedback messgaes. By default this feature is 
      * enabled.
      */
    void disable_watchdog(void);
    /**
      * \brief Checks whether the feedback watchdog is enabled or not
      * 
      * This functions checks whether the internal feedback watchdog for the current
      * action is enabled or not.
326
      *
327
328
      * \return A boolean indicating whether the watchdog is enabled (true) or not
      *         (false)
329
      */
330
    bool is_watchdog_enabled(void);
331
332
333
334
335
336
337
338
339
340
    /**
      * \brief Gets the feedback watchdog time
      * 
      * This functions returns the maximum allowed time between two consecutive 
      * feedback messages before reporting an error. By default this value is
      * set to 2.
      *
      * \return the maximum allowed time in seconds between two consecutive 
      *         feedback messages.
      */
341
    double get_feedback_watchdog_time(void);
342
343
344
345
346
347
348
349
350
351
    /**
      * \brief Checks the state of the watchdog
      * 
      * This function checks whether the internal watchdog has been activated
      * because the feedback callback functions has not been called for longer
      * than the allowed time.
      *
      * \return A boolean indicating whether the watchdog is active (true) or
      *         not (false)
      */
352
    bool is_watchdog_active(void);
353
354
355
356
357
358
    /**
      * \brief Enables the action timeout
      * 
      * This function enables the internal action timeout to limit the maximum 
      * time the action can be active without finishing properly. By default 
      * this feature is disabled.
359
360
361
      *
      * \param time_s the desired maximum ammount of time the action can be 
      *               active before reporting and error.
362
      */
363
    void enable_timeout(double time_s);
364
365
366
367
368
369
370
    /**
      * \brief Disables the action timeout
      * 
      * This function disables the internal action timeout to limit the maximum 
      * time the action can be active without finishing properly. By default 
      * this feature is disabled.
      */
371
    void disable_timeout(void); 
372
373
374
375
376
377
378
379
380
381
382
    /**
      * \brief Updates the current timeout value
      * 
      * This function updates the current timeout value, if it is enabled. The
      * time already elapsed since the start of the action is not taken into
      * account when updating the value, so the provided time will be the new
      * timeout time.
      *
      * \param time_s the new timeout time in seconds to wait for the termination
      *               of the current action.
      */
383
    void update_timeout(double time_s);
384
385
386
387
388
389
    /**
      * \brief Stops the timeout
      * 
      * This functions stops the internal timeout, if it is enabled, so that no
      * timeout error will be reported.
      */
390
    void stop_timeout(void);
391
392
393
394
395
396
397
398
399
    /**
      * \brief Checks whether the timeout is enabled or not
      * 
      * This functions checks whether the internal timeout value for the current
      * action is enabled or not.
      *
      * \return A boolean indicating whether the timeout is enabled (true) or not
      *         (false)
      */
400
    bool is_timeout_enabled(void);
401
402
403
404
405
406
407
408
409
    /**
      * \brief Checks whether the timeout is active or not
      * 
      * This functions chekcs whether the maximum time allowed to complete the 
      *action has elapsed or not. 
      * 
      * \return A boolean indicating whether the timeout is active (true) or not
      *         (false)
      */
410
    bool is_timeout_active(void);
411
412
413
414
415
416
417
418
    /**
      * \brief Function to get the name of the module
      *
      * This function returns the full name of the module, with the whole namespace.
      *
      * \return the name of the module.
      * 
      */
419
    std::string get_name(void);
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
    /**
      * \brief 
      *
      */
    void enable(void);
    /**
      * \brief 
      *
      */
    void disable(void);
    /**
      * \brief 
      *
      */
    bool is_enabled(void);
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
    /**
      * \brief start the action
      * 
      * This function start the action with the provided goal information. This function
      * first checks whether the action server is connected or not. In case it is not 
      * connected it will return reporting a pending condition util the maximum number of
      * repetitions has been reached. In this case it will report an error.
      *
      * If the action server is connected, it sends all the goal information, resets the 
      * feeedback watchdog and enables the internal timeout in case it is enabled.
      *
      * \param msg action dependant goal data structure to be send to the action server.
      *
      * \return the status of the request. It can be any value of the act_srv_status
      *         data type.
      */
451
452
    act_srv_status make_request(Goal &msg)
    {
453
      if(this->enabled)
454
      {
455
        if(this->action_client->isServerConnected())
456
        {
457
          ROS_DEBUG_STREAM("CModuleAction::make_request: Server " << this->name << " is Available!");
458
          this->current_num_retries=0;
459
460
461
462
463
464
465
466
467
468
469
470
471
          this->action_client->sendGoal(msg,
                      boost::bind(&CModuleAction<action_ros>::action_done,     this, _1, _2),
                      boost::bind(&CModuleAction<action_ros>::action_active,   this),
                      boost::bind(&CModuleAction<action_ros>::action_feedback, this, _1));
          this->feedback_watchdog.reset(ros::Duration(this->watchdog_time));
          this->status=ACTION_RUNNING;
          if(this->use_timeout)
          {
            if(this->timeout_value>0.0)
              this->action_timeout.start(ros::Duration(this->timeout_value));
          }
          ROS_DEBUG_STREAM("CModuleAction::make_request: Goal Sent to server " << this->name << ". Wait for Result!");
          return ACT_SRV_SUCCESS;
472
473
474
        }
        else
        {
475
476
477
478
479
480
481
482
483
484
485
486
          this->current_num_retries++;
          if(this->current_num_retries>this->max_num_retries)
          {
            this->current_num_retries=0;
            ROS_DEBUG_STREAM("CModuleAction::make_request: Action start on server " << this->name << " failed!");
            return ACT_SRV_FAIL;
          }
          else
          {
            ROS_DEBUG_STREAM("CModuleAction::make_request: Action start on server " << this->name << " pending!");
            return ACT_SRV_PENDING;
          }
487
488
        }
      }
489
490
491
492
493
      else 
      {
        this->status=ACTION_RUNNING;
        return ACT_SRV_SUCCESS;
      }
494
495
    }

496
497
498
499
500
501
502
503
    /**
      * \brief Cancels the current action
      * 
      * This function cancels the current action. When this function is called,
      * the action in the action server may still be active, so it is necessary 
      * to check the is_finished() function to actually know when the action
      * has ended.
      */
504
    void cancel(void);
505
506
507
508
509
510
511
512
513
514
    /**
      * \brief Checks whether the action has finished or not
      * 
      * This function checks whether the current action has ended or not. The
      * action may end for several reasons, use the get_state() function to
      * know the exact cause.
      * 
      * \return A boolean indicating whether the current actions has finished
      *         (true) or not (false);
      */
515
    bool is_finished(void);
516
517
518
519
520
521
522
523
524
    /**
      * \brief Returns the current status of the action
      * 
      * This functions returns the current status of the action. If no action 
      * is active, the value returned by this function is meaningless.
      *  
      * \return the current state of the action. It can be any value of the 
      *         actio_status data structure.
      */
525
    action_status get_state(void);
526
527
528
529
530
531
532
533
534
535
536
    /**
      * \brief Returns the action result
      * 
      * This function returns the action dependant result data structure 
      * returned by the action server when the last goal has ended. If no 
      * goal has been started of the current goal is still active, the
      * value returned by this function is meaningless.
      *
      * \return an action dependant result data structure of the last 
      *         executed action.
      */
537
538
539
540
    Result get_result(void)
    {
      return this->action_result_msg;
    }
541
542
543
544
545
546
547
548
549
550
    /**
      * \brief Returns the action feedback
      * 
      * This function returns the last received action dependant feedback
      * data structure. If no action is active, the value resturned by
      * this functions is meaningless.
      *
      * \return the last action dependant feedback data structure received
      *         from the current goal.
      */
551
552
553
554
    Feedback get_feedback(void)
    {
      return this->action_feedback_msg;
    }
555
556
557
558
    /**
      * \brief Destructor
      * 
      */
559
560
561
562
    ~CModuleAction();
};

template<class action_ros>
563
CModuleAction<action_ros>::CModuleAction(const std::string &name,const std::string &name_space):nh(name_space)
564
{
565
  this->name=this->nh.getNamespace()+"/"+name;
566
567
568
569
570
571
572
  this->status=ACTION_IDLE;
  // retry control
  this->current_num_retries=0;
  this->max_num_retries=DEFAULT_ACTION_MAX_RETRIES;
  // assign the full service name
  // timeouts
  this->use_timeout=false;
573
  this->timeout_value=0.0;
574
575
  this->timeout_value=DEFAULT_ACTION_TIMEOUT;
  // waychdog
576
  this->use_watchdog=true;
577
578
579
580
  this->watchdog_time=DEFAULT_WATCHDOG_TIME;
  // internal action
  this->action_client=NULL;
  this->action_client=new actionlib::SimpleActionClient<action_ros>(this->nh,name,true);
581
582
  // enable parameter
  this->enabled=true;
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
}

template<class action_ros>
void CModuleAction<action_ros>::action_active(void)
{
  ROS_DEBUG_STREAM("CModuleAction::action_active: Goal on server " << this->name << " just went active!");
}

template<class action_ros>
void CModuleAction<action_ros>::set_max_num_retries(unsigned int max_retries)
{
  this->action_access.enter();
  this->max_num_retries=max_retries;
  this->action_access.exit();
}

template<class action_ros>
unsigned int CModuleAction<action_ros>::get_max_num_retries(void)
{
  return this->max_num_retries;
}

template<class action_ros>
606
void CModuleAction<action_ros>::enable_watchdog(double time_s)
607
608
609
{
  this->action_access.enter();
  if(time_s>0.0)
610
  {
611
    this->watchdog_time=time_s;
612
613
614
    this->use_watchdog=true;
    this->feedback_watchdog.reset(ros::Duration(this->watchdog_time));
  }
615
616
617
  else
  {
    this->action_access.exit();
618
    throw CModuleException(_HERE_,"Invalid watchdog value",this->name);
619
  }
620
621
622
  this->action_access.exit();
}

623
624
625
626
627
628
629
630
631
632
633
634
635
636
template<class action_ros>
void CModuleAction<action_ros>::disable_watchdog(void)
{
  this->action_access.enter();
  this->use_watchdog=false;
  this->action_access.exit();
}

template<class action_ros>
bool CModuleAction<action_ros>::is_watchdog_enabled(void)
{
  return this->use_watchdog;
}

637
638
639
640
641
642
643
644
645
646
template<class action_ros>
double CModuleAction<action_ros>::get_feedback_watchdog_time(void)
{
  return this->watchdog_time;
}

template<class action_ros>
bool CModuleAction<action_ros>::is_watchdog_active(void)
{
  this->action_access.enter();
647
  if(this->enabled)
648
  {
649
    if(this->use_watchdog && this->feedback_watchdog.is_active())
650
651
652
653
654
655
656
657
658
659
    {
      this->status=ACTION_FB_WATCHDOG;
      this->action_access.exit();
      return true;
    }
    else
    {
      this->action_access.exit();
      return false;
    }
660
661
662
663
664
665
  }
  else
    return false;
}

template<class action_ros>
666
void CModuleAction<action_ros>::enable_timeout(double time_s)
667
668
{
  this->action_access.enter();
669
670
671
672
673
674
675
676
677
678
  if(time_s>0.0)
  {
    this->timeout_value=time_s;
    this->use_timeout=true;
  }
  else
  {
    this->action_access.exit();
    throw CModuleException(_HERE_,"Invalid timeout value",this->name);
  }
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
  this->action_access.exit();
}

template<class action_ros>
void CModuleAction<action_ros>::disable_timeout(void)
{
  this->action_access.enter();
  this->use_timeout=false;
  this->action_access.exit();
}

template<class action_ros>
void CModuleAction<action_ros>::update_timeout(double time_s)
{
  this->action_access.enter();
694
  if(this->use_timeout)
695
  {
696
697
698
699
700
701
702
703
704
705
    if(time_s>0.0)
    {
      this->action_timeout.start(ros::Duration(time_s));
      this->timeout_value=time_s;
    }
    else
    {
      this->action_access.exit();
      throw CModuleException(_HERE_,"Invalid timeout value",this->name);
    }
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
  }
  this->action_access.exit();
}

template<class action_ros>
void CModuleAction<action_ros>::stop_timeout(void)
{
  this->action_access.enter();
  this->action_timeout.stop();
  this->action_access.exit();
}

template<class action_ros>
bool CModuleAction<action_ros>::is_timeout_enabled(void)
{
  return this->use_timeout;
}

template<class action_ros>
bool CModuleAction<action_ros>::is_timeout_active(void)
{
  this->action_access.enter();
728
  if(this->use_timeout && this->action_timeout.timed_out())
729
730
731
732
733
734
735
736
737
738
739
740
  {
    this->status=ACTION_TIMEOUT;
    this->action_access.exit();
    return true;
  }
  else
  {
    this->action_access.exit();
    return false;
  }
}

741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
template<class action_ros>
void CModuleAction<action_ros>::enable(void)
{
  this->action_access.enter();
  this->enabled=true;
  this->action_access.exit();
}

template<class action_ros>
void CModuleAction<action_ros>::disable(void)
{
  this->action_access.enter();
  this->enabled=false;
  this->action_access.exit();
}

template<class action_ros>
bool CModuleAction<action_ros>::is_enabled(void)
{
  return this->enabled;
}

763
764
765
766
767
768
769
770
771
template<class action_ros>
std::string CModuleAction<action_ros>::get_name(void)
{
  return this->name;
}

template<class action_ros>
void CModuleAction<action_ros>::cancel(void)
{
772
  actionlib::SimpleClientGoalState action_state(actionlib::SimpleClientGoalState::PENDING);
773
774
775
776
777
778
779

  if(this->enabled)
  { 
    action_state=action_client->getState();
    if(action_state==actionlib::SimpleClientGoalState::ACTIVE)
      this->action_client->cancelGoal();
  }
780
781
782
783
784
785
}

template<class action_ros>
bool CModuleAction<action_ros>::is_finished(void)
{
  actionlib::SimpleClientGoalState action_state(actionlib::SimpleClientGoalState::PENDING);
786
  
787
788
789
790
791
792
793
794
795
  if(this->enabled)
  {
    action_state=action_client->getState();
    if(action_state==actionlib::SimpleClientGoalState::ACTIVE)
      return false;
    else
      return true;
  }
  else 
796
797
798
799
800
801
    return true;
}

template<class action_ros>
action_status CModuleAction<action_ros>::get_state(void)
{
802
  if(this->enabled)
803
  {
804
805
    if(this->status==ACTION_RUNNING)
    {
806
807
      this->is_timeout_active();
      this->is_watchdog_active();
808
    }
809
  }
810
811
  else
    this->status=ACTION_SUCCESS;
812
813
814
815
816
817
818

  return this->status;
}

template<class action_ros>
CModuleAction<action_ros>::~CModuleAction()
{
819
  if(this->enabled)
820
  {
821
822
823
824
825
    if(this->status==ACTION_RUNNING)
    {
      this->action_client->cancelGoal();
      while(!this->is_finished());
    }
826
827
828
829
830
  }
  delete this->action_client;
}

#endif