From 2c46daf712de1e2db067eb32f923881534a66b52 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Fri, 22 Oct 2021 10:56:59 +0200
Subject: [PATCH] Add block params
---
cfg/AdcJuryServer.cfg | 4 ++++
src/adc_jury_server_alg_node.cpp | 12 +++++++++++-
2 files changed, 15 insertions(+), 1 deletion(-)
diff --git a/cfg/AdcJuryServer.cfg b/cfg/AdcJuryServer.cfg
index 2257b91..9c8ea6a 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 74bc90d..f739613 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();
}
--
GitLab