diff --git a/include/iri_ros_tools/module_action.h b/include/iri_ros_tools/module_action.h index 3ed6bc3292fb3cdc5ee674eaa4d9d2f47c63ea91..48fce49dad36665d93293a4e580f53ddc37f4d72 100644 --- a/include/iri_ros_tools/module_action.h +++ b/include/iri_ros_tools/module_action.h @@ -155,6 +155,11 @@ class CModuleAction * further details. */ CROSWatchdog feedback_watchdog; + /** + * \brief + * + */ + bool enabled; // action attributes /** * \brief ROS topic messages names @@ -381,6 +386,21 @@ class CModuleAction * */ std::string get_name(void); + /** + * \brief + * + */ + void enable(void); + /** + * \brief + * + */ + void disable(void); + /** + * \brief + * + */ + bool is_enabled(void); /** * \brief start the action * @@ -399,39 +419,47 @@ class CModuleAction */ act_srv_status make_request(Goal &msg) { - if(this->action_client->isServerConnected()) - { - ROS_DEBUG_STREAM("CModuleAction::make_request: Server " << this->name << " is Available!"); - this->current_num_retries=0; - 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; - } - else + if(this->enabled) { - this->current_num_retries++; - if(this->current_num_retries>this->max_num_retries) + if(this->action_client->isServerConnected()) { + ROS_DEBUG_STREAM("CModuleAction::make_request: Server " << this->name << " is Available!"); this->current_num_retries=0; - ROS_DEBUG_STREAM("CModuleAction::make_request: Action start on server " << this->name << " failed!"); - return ACT_SRV_FAIL; + 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; } else { - ROS_DEBUG_STREAM("CModuleAction::make_request: Action start on server " << this->name << " pending!"); - return ACT_SRV_PENDING; + 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; + } } } + else + { + this->status=ACTION_RUNNING; + return ACT_SRV_SUCCESS; + } } /** @@ -518,6 +546,8 @@ CModuleAction<action_ros>::CModuleAction(const std::string &name,const std::stri // internal action this->action_client=NULL; this->action_client=new actionlib::SimpleActionClient<action_ros>(this->nh,name,true); + // enable parameter + this->enabled=true; } template<class action_ros> @@ -564,17 +594,22 @@ template<class action_ros> bool CModuleAction<action_ros>::is_watchdog_active(void) { this->action_access.enter(); - if(this->feedback_watchdog.is_active()) + if(this->enabled) { - this->status=ACTION_FB_WATCHDOG; - this->action_access.exit(); - return true; + if(this->feedback_watchdog.is_active()) + { + this->status=ACTION_FB_WATCHDOG; + this->action_access.exit(); + return true; + } + else + { + this->action_access.exit(); + return false; + } } else - { - this->action_access.exit(); return false; - } } template<class action_ros> @@ -653,6 +688,28 @@ bool CModuleAction<action_ros>::is_timeout_active(void) } } +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; +} + template<class action_ros> std::string CModuleAction<action_ros>::get_name(void) { @@ -663,10 +720,13 @@ template<class action_ros> void CModuleAction<action_ros>::cancel(void) { actionlib::SimpleClientGoalState action_state(actionlib::SimpleClientGoalState::PENDING); - - action_state=action_client->getState(); - if(action_state==actionlib::SimpleClientGoalState::ACTIVE) - this->action_client->cancelGoal(); + + if(this->enabled) + { + action_state=action_client->getState(); + if(action_state==actionlib::SimpleClientGoalState::ACTIVE) + this->action_client->cancelGoal(); + } } template<class action_ros> @@ -674,23 +734,33 @@ bool CModuleAction<action_ros>::is_finished(void) { actionlib::SimpleClientGoalState action_state(actionlib::SimpleClientGoalState::PENDING); - action_state=action_client->getState(); - if(action_state==actionlib::SimpleClientGoalState::ACTIVE) - return false; - else + if(this->enabled) + { + action_state=action_client->getState(); + if(action_state==actionlib::SimpleClientGoalState::ACTIVE) + return false; + else + return true; + } + else return true; } template<class action_ros> action_status CModuleAction<action_ros>::get_state(void) { - if(this->status==ACTION_RUNNING) + if(this->enabled) { - if(this->use_timeout && this->is_timeout_active()) - this->status=ACTION_TIMEOUT; - if(this->is_watchdog_active()) - this->status=ACTION_FB_WATCHDOG; + if(this->status==ACTION_RUNNING) + { + if(this->use_timeout && this->is_timeout_active()) + this->status=ACTION_TIMEOUT; + if(this->is_watchdog_active()) + this->status=ACTION_FB_WATCHDOG; + } } + else + this->status=ACTION_SUCCESS; return this->status; } @@ -698,10 +768,13 @@ action_status CModuleAction<action_ros>::get_state(void) template<class action_ros> CModuleAction<action_ros>::~CModuleAction() { - if(this->status==ACTION_RUNNING) + if(this->enabled) { - this->action_client->cancelGoal(); - while(!this->is_finished()); + if(this->status==ACTION_RUNNING) + { + this->action_client->cancelGoal(); + while(!this->is_finished()); + } } delete this->action_client; } diff --git a/include/iri_ros_tools/module_service.h b/include/iri_ros_tools/module_service.h index d8d1aaa3ef2b61c672aa553098939e2cd8a219de..ab94cd00e6f2c8dd6c472cea21b99584a6561e5e 100644 --- a/include/iri_ros_tools/module_service.h +++ b/include/iri_ros_tools/module_service.h @@ -55,6 +55,11 @@ class CModuleService * */ unsigned int max_num_retries; + /** + * \brief + * + */ + bool enabled; /** * \brief internal ROS node handle * @@ -136,6 +141,21 @@ class CModuleService * */ std::string get_name(void); + /** + * \brief + * + */ + void enable(void); + /** + * \brief + * + */ + void disable(void); + /** + * \brief + * + */ + bool is_enabled(void); /** * \brief Sets the callback function * @@ -178,6 +198,8 @@ CModuleService<service_msg>::CModuleService(const std::string &name,const std::s this->name=this->nh.getNamespace()+"/"+name; // assign the default check function this->call_successfull=boost::bind(&CModuleService::default_call_check, this, _1); + // enable parameter + this->enabled=true; } template<class service_msg> @@ -204,18 +226,54 @@ std::string CModuleService<service_msg>::get_name(void) return this->name; } +template<class service_msg> +void CModuleService<service_msg>::enable(void) +{ + this->enabled=true; +} + +template<class service_msg> +void CModuleService<service_msg>::disable(void) +{ + this->enabled=false; +} + +template<class service_msg> +bool CModuleService<service_msg>::is_enabled(void) +{ + return this->enabled; +} + template<class service_msg> act_srv_status CModuleService<service_msg>::call(service_msg &msg) { ROS_DEBUG_STREAM("CModuleService::call: Sending New Request to service " << this->service_client.getService()); - if(this->service_client.call(msg)) + if(this->enabled) { - if(this->call_successfull(msg)) + if(this->service_client.call(msg)) { - ROS_DEBUG_STREAM("CModuleService::call: Request successfull! (server: " << this->service_client.getService() << ")"); - this->current_num_retries=0; - return ACT_SRV_SUCCESS; + if(this->call_successfull(msg)) + { + ROS_DEBUG_STREAM("CModuleService::call: Request successfull! (server: " << this->service_client.getService() << ")"); + this->current_num_retries=0; + return ACT_SRV_SUCCESS; + } + else + { + this->current_num_retries++; + if(this->current_num_retries>this->max_num_retries) + { + ROS_ERROR_STREAM("CModuleService::call: Request failed! (server: " << this->service_client.getService() << ")"); + this->current_num_retries=0; + return ACT_SRV_FAIL; + } + else + { + ROS_WARN_STREAM("CModuleService::call: Request pending! (server: " << this->service_client.getService() << ")"); + return ACT_SRV_PENDING; + } + } } else { @@ -234,20 +292,7 @@ act_srv_status CModuleService<service_msg>::call(service_msg &msg) } } else - { - this->current_num_retries++; - if(this->current_num_retries>this->max_num_retries) - { - ROS_ERROR_STREAM("CModuleService::call: Request failed! (server: " << this->service_client.getService() << ")"); - this->current_num_retries=0; - return ACT_SRV_FAIL; - } - else - { - ROS_WARN_STREAM("CModuleService::call: Request pending! (server: " << this->service_client.getService() << ")"); - return ACT_SRV_PENDING; - } - } + return ACT_SRV_SUCCESS; } template<class service_msg>