Skip to content

Commit 1fe95eb

Browse files
randaz81randaz81
authored
Merge pull request #19 from fbrand-new/fix/service_delay
Fix service client delay by opening it in skill start
2 parents 71b4dd1 + 1065bea commit 1fe95eb

File tree

4 files changed

+62
-38
lines changed

4 files changed

+62
-38
lines changed

‎include/Data.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -133,6 +133,7 @@ struct savedCodeStr{
133133
std::string topicCallbackH;
134134
std::string topicInterfaceH;
135135
std::string topicParamList;
136+
std::string serviceClientH;
136137
std::string actionC;
137138
std::string actionH;
138139
std::string actionInterfaceH;

‎src/Replacer.cpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -316,6 +316,11 @@ void handleGenericEvent(const eventDataStr eventData, const savedCodeStr savedCo
316316

317317
writeAfterCommand(str, "/*SEND_EVENT_LIST*/", eventCodeC);
318318

319+
// Service client header code
320+
std::string serviceClientH = savedCode.serviceClientH;
321+
replaceCommonEventPlaceholders(serviceClientH, eventData);
322+
writeAfterCommand(str, "/*SERVICE_CLIENTS_LIST*/", serviceClientH);
323+
319324
//H
320325
replaceAll(interfaceCodeH, "$eventData.interfaceName$", eventData.interfaceName);
321326
replaceAll(interfaceCodeH, "$eventData.functionNameSnakeCase$", eventData.functionNameSnakeCase);
@@ -609,6 +614,8 @@ void saveCode(savedCodeStr& savedCode, std::string& code)
609614
deleteSection(code, "/*TOPIC_CALLBACK_H*/", "/*END_TOPIC_CALLBACK_H*/");
610615
saveSection(code, "/*TOPIC_SUBSCRIPTION_H*/", "/*END_TOPIC_SUBSCRIPTION_H*/", savedCode.topicSubscriptionH);
611616
deleteSection(code, "/*TOPIC_SUBSCRIPTION_H*/", "/*END_TOPIC_SUBSCRIPTION_H*/");
617+
saveSection(code, "/*SERVICE_CLIENT*/", "/*END_SERVICE_CLIENT*/", savedCode.serviceClientH);
618+
deleteSection(code, "/*SERVICE_CLIENT*/", "/*END_SERVICE_CLIENT*/");
612619
saveSection(code, "/*ACTION_H*/", "/*END_ACTION_H*/", savedCode.actionH);
613620
deleteSection(code, "/*ACTION_H*/", "/*END_ACTION_H*/");
614621
saveSection(code, "/*ACTION_INTERFACE*/", "/*END_ACTION_INTERFACE*/", savedCode.actionInterfaceH);
@@ -663,6 +670,7 @@ void replaceEventCode(std::map <std::string, std::string>& codeMap, fileDataStr
663670
deleteCommand(it->second, "/*ACTION_FNC_LIST*/");
664671
//H
665672
deleteCommand(it->second, "/*INTERFACES_LIST*/");
673+
deleteCommand(it->second, "/*SERVICE_CLIENTS_LIST*/");
666674
deleteCommand(it->second, "/*TOPIC_SUBSCRIPTIONS_LIST_H*/");
667675
deleteCommand(it->second, "/*TOPIC_CALLBACK_LIST_H*/");
668676
deleteCommand(it->second, "/*ACTION_LIST_H*/");

‎template_skill/include/TemplateSkill.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,9 @@ class $className$
6161
/*TOPIC_SUBSCRIPTIONS_LIST_H*/
6262
/*TOPIC_SUBSCRIPTION_H*/
6363
rclcpp::Subscription<$eventData.interfaceName$::msg::$eventData.messageNameSnakeCase$>::SharedPtr m_subscription_$eventData.functionName$;/*END_TOPIC_SUBSCRIPTION_H*/
64+
/*SERVICE_CLIENTS_LIST*//*SERVICE_CLIENT*/
65+
std::shared_ptr<rclcpp::Node> $eventData.nodeName$;
66+
std::shared_ptr<rclcpp::Client<$eventData.interfaceName$::srv::$eventData.functionName$>> $eventData.clientName$;/*END_SERVICE_CLIENT*/
6467
/*ACTION_LIST_H*//*ACTION_H*/
6568
std::shared_ptr<rclcpp::Node> m_node_action;
6669
std::mutex m_actionMutex;

‎template_skill/src/TemplateSkill.cpp‎

Lines changed: 50 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77
#include <QTime>
88
#include <iostream>
99
#include <QStateMachine>
10-
10+
#include <cstdlib>
1111
#include <type_traits>
1212

1313
template<typename T>
@@ -92,47 +92,53 @@ bool $className$::start(int argc, char*argv[])
9292
"$eventData.topicName$", 10, std::bind(&$className$::topic_callback_$eventData.functionName$, this, std::placeholders::_1));
9393
/*END_TOPIC_SUBSCRIPTION*/
9494
/*SEND_EVENT_LIST*//*SEND_EVENT_SRV*/
95+
$eventData.nodeName$ = rclcpp::Node::make_shared(m_name + "SkillNode$eventData.functionName$");
96+
$eventData.clientName$ = $eventData.nodeName$->create_client<$eventData.interfaceName$::srv::$eventData.functionName$>($eventData.serverName$);
97+
$eventData.clientName$->configure_introspection($eventData.nodeName$->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
98+
99+
bool wait_succeded{true};
100+
int retries = 0;
101+
while (!$eventData.clientName$->wait_for_service(std::chrono::seconds(1))) {
102+
if (!rclcpp::ok()) {
103+
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service '$eventData.functionName$'. Exiting.");
104+
wait_succeded = false;
105+
break;
106+
}
107+
retries++;
108+
if(retries == SERVICE_TIMEOUT) {
109+
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Timed out while waiting for the service '$eventData.functionName$'.");
110+
wait_succeded = false;
111+
break;
112+
}
113+
}
114+
if (!wait_succeded) {
115+
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service '$eventData.componentName$/$eventData.functionName$' not available.");
116+
std::exit(1);
117+
}
118+
/*SEND_EVENT_LIST*//*SEND_EVENT_SRV*/
95119
m_stateMachine.connectToEvent("$eventData.event$", [this]([[maybe_unused]]const QScxmlEvent & event){
96-
std::shared_ptr<rclcpp::Node> $eventData.nodeName$ = rclcpp::Node::make_shared(m_name + "SkillNode$eventData.functionName$");
97-
std::shared_ptr<rclcpp::Client<$eventData.interfaceName$::srv::$eventData.serviceTypeName$>> $eventData.clientName$ = $eventData.nodeName$->create_client<$eventData.interfaceName$::srv::$eventData.serviceTypeName$>($eventData.serverName$);
98-
auto request = std::make_shared<$eventData.interfaceName$::srv::$eventData.serviceTypeName$::Request>();
99-
$eventData.clientName$->configure_introspection($eventData.nodeName$->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS);
120+
auto request = std::make_shared<$eventData.interfaceName$::srv::$eventData.functionName$::Request>();
100121
auto eventParams = event.data().toMap();
101122
/*PARAM_LIST*//*PARAM*/
102123
request->$IT->FIRST$ = convert<decltype(request->$IT->FIRST$)>(eventParams["$IT->FIRST$"].toString().toStdString());/*END_PARAM*/
103-
bool wait_succeded{true};
104-
int retries = 0;
105-
while (!$eventData.clientName$->wait_for_service(std::chrono::seconds(1))) {
106-
if (!rclcpp::ok()) {
107-
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service '$eventData.functionName$'. Exiting.");
108-
wait_succeded = false;
109-
break;
110-
}
111-
retries++;
112-
if(retries == SERVICE_TIMEOUT) {
113-
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Timed out while waiting for the service '$eventData.functionName$'.");
114-
wait_succeded = false;
115-
break;
116-
}
124+
auto result = $eventData.clientName$->async_send_request(request);
125+
const std::chrono::seconds timeout_duration(SERVICE_TIMEOUT);
126+
auto futureResult = rclcpp::spin_until_future_complete($eventData.nodeName$, result, timeout_duration);
127+
if (futureResult == rclcpp::FutureReturnCode::SUCCESS)
128+
{
129+
auto response = result.get();
130+
QVariantMap data;
131+
data.insert("call_succeeded", true);/*RETURN_PARAM_LIST*//*RETURN_PARAM*/
132+
data.insert("$eventData.interfaceDataField$", response->$eventData.interfaceDataField$);/*END_RETURN_PARAM*/
133+
m_stateMachine.submitEvent("$eventData.componentName$.$eventData.functionName$.Return", data);
134+
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "$eventData.componentName$.$eventData.functionName$.Return");
135+
return;
136+
}
137+
else if(futureResult == rclcpp::FutureReturnCode::TIMEOUT){
138+
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Timed out while future complete for the service '$eventData.functionName$'.");
117139
}
118-
if (wait_succeded) {
119-
auto result = $eventData.clientName$->async_send_request(request);
120-
const std::chrono::seconds timeout_duration(SERVICE_TIMEOUT);
121-
auto futureResult = rclcpp::spin_until_future_complete($eventData.nodeName$, result, timeout_duration);
122-
if (futureResult == rclcpp::FutureReturnCode::SUCCESS)
123-
{
124-
auto response = result.get();
125-
QVariantMap data;
126-
data.insert("call_succeeded", true);/*RETURN_PARAM_LIST*//*RETURN_PARAM*/
127-
data.insert("$eventData.interfaceDataField$", response->$eventData.interfaceDataField$);/*END_RETURN_PARAM*/
128-
m_stateMachine.submitEvent("$eventData.componentName$.$eventData.functionName$.Return", data);
129-
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "$eventData.componentName$.$eventData.functionName$.Return");
130-
return;
131-
132-
}
133-
else if(futureResult == rclcpp::FutureReturnCode::TIMEOUT){
134-
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Timed out while future complete for the service '$eventData.functionName$'.");
135-
}
140+
else {
141+
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service '$eventData.functionName$'.");
136142
}
137143
QVariantMap data;
138144
data.insert("call_succeeded", false);
@@ -212,9 +218,14 @@ void $className$::tick( [[maybe_unused]] const std::shared_ptr<bt_interfaces_dum
212218
m_tickResult.store(Status::undefined);
213219
m_stateMachine.submitEvent("CMD_TICK");
214220

221+
int load_counter=0;
222+
auto start_timer = std::chrono::steady_clock::now();
215223
while(m_tickResult.load()== Status::undefined) {
216-
std::this_thread::sleep_for (std::chrono::milliseconds(100));
224+
std::this_thread::sleep_for (std::chrono::milliseconds(5));
225+
load_counter++;
217226
}
227+
auto end_timer = std::chrono::steady_clock::now();
228+
auto duration_ms = std::chrono::duration_cast<std::chrono::milliseconds>(end_timer - start_timer).count();
218229
switch(m_tickResult.load())
219230
{
220231
/*ACTION*/case Status::running:
@@ -231,6 +242,7 @@ void $className$::tick( [[maybe_unused]] const std::shared_ptr<bt_interfaces_dum
231242
break;
232243
}
233244
RCLCPP_INFO(m_node->get_logger(), "$className$::tickDone");
245+
RCLCPP_DEBUG(m_node->get_logger(), "$className$ num_retry: %d tick time: %ld", load_counter, duration_ms);
234246
response->is_ok = true;
235247
}/*END_TICK_CMD*/
236248
/*HALT_CMD*/

0 commit comments

Comments
 (0)