diff --git a/cfg/AdcJuryServer.cfg b/cfg/AdcJuryServer.cfg index 2257b9186b1012e21c6882c7e1135238a140a461..9c8ea6a1a71d9d4c90b2de10b9018a310dd5d228 100755 --- a/cfg/AdcJuryServer.cfg +++ b/cfg/AdcJuryServer.cfg @@ -42,6 +42,10 @@ gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 10 gen.add("feedback_status_text", str_t, 0, "Feedback text", "feedback text example") gen.add("finish_action_with_success", bool_t, 0, "End action with success", False) gen.add("finish_action_with_failure", bool_t, 0, "End action with failure", False) +gen.add("block_feedback", bool_t, 0, "Block action feedback function", True) +gen.add("block_feedback_time", double_t, 0, "Time in seconds to block action feedback function", 1.0, 0.0, 10.0) +gen.add("block_service", bool_t, 0, "Block service callback function", True) +gen.add("block_service_time", double_t, 0, "Time in seconds to block service callback function", 3.0, 0.0, 10.0) exit(gen.generate(PACKAGE, "AdcJuryServerAlgorithm", "AdcJuryServer")) diff --git a/src/adc_jury_server_alg_node.cpp b/src/adc_jury_server_alg_node.cpp index 74bc90d0bda320665e001878f53ca76a509ac813..f739613cad4d757748ebc9269c8a7f0681e1fa8a 100644 --- a/src/adc_jury_server_alg_node.cpp +++ b/src/adc_jury_server_alg_node.cpp @@ -85,6 +85,10 @@ bool AdcJuryServerAlgNode::jury_modeCallback(iri_adc_msgs::jury_mode::Request &r ROS_INFO("AdcJuryServerAlgNode::jury_modeCallback: req=%d!",req.enable); //do operations with req and output on res + + if(this->config_.block_service) + sleep(this->config_.block_service_time); + //res.data2 = req.data1 + my_var; //unlock previously blocked shared variables @@ -164,9 +168,15 @@ void AdcJuryServerAlgNode::adc_juryGetFeedbackCallback(iri_adc_msgs::adc_juryFee { this->alg_.lock(); //update feedback data to be sent to client - feedback->status = this->config_.feedback_status_text; + static int i=0; + feedback->status = this->config_.feedback_status_text + std::to_string(i);; + i++; //ROS_INFO("feedback: %s", feedback->data.c_str()); //ROS_INFO("AdcJuryServerAlgNode::adc_juryGetFeedbackCallback feedback.status='%s'", feedback->status.c_str()); + if(this->config_.block_feedback) + { + sleep(this->config_.block_feedback_time); + } this->alg_.unlock(); }