Skip to content

Commit bc5297e

Browse files
facontidavidefacontidavide
committed
make it easier to change action/service name programmatically
1 parent 0582a86 commit bc5297e

File tree

6 files changed

+106
-155
lines changed

6 files changed

+106
-155
lines changed

‎behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp‎

Lines changed: 33 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -108,8 +108,7 @@ class RosActionNode : public BT::ActionNodeBase
108108
*/
109109
static PortsList providedBasicPorts(PortsList addition)
110110
{
111-
PortsList basic = { InputPort<std::string>("action_name", "__default__placeholder__",
112-
"Action server name") };
111+
PortsList basic = { InputPort<std::string>("action_name", "", "Action server name") };
113112
basic.insert(addition.begin(), addition.end());
114113
return basic;
115114
}
@@ -164,9 +163,12 @@ class RosActionNode : public BT::ActionNodeBase
164163
void cancelGoal();
165164

166165
/// The default halt() implementation will call cancelGoal if necessary.
167-
void halt() override final;
166+
void halt() override;
168167

169-
NodeStatus tick() override final;
168+
NodeStatus tick() override;
169+
170+
/// Can be used to change the name of the action programmatically
171+
void setActionName(const std::string& action_name);
170172

171173
protected:
172174
struct ActionClientInstance
@@ -216,7 +218,7 @@ class RosActionNode : public BT::ActionNodeBase
216218
std::weak_ptr<rclcpp::Node> node_;
217219
std::shared_ptr<ActionClientInstance> client_instance_;
218220
std::string action_name_;
219-
bool action_name_may_change_ = false;
221+
bool action_name_should_be_checked_ = false;
220222
const std::chrono::milliseconds server_timeout_;
221223
const std::chrono::milliseconds wait_for_server_timeout_;
222224
std::string action_client_key_;
@@ -265,44 +267,23 @@ inline RosActionNode<T>::RosActionNode(const std::string& instance_name,
265267
auto portIt = config().input_ports.find("action_name");
266268
if(portIt != config().input_ports.end())
267269
{
268-
const std::string& bb_action_name = portIt->second;
270+
const std::string& bb_service_name = portIt->second;
269271

270-
if(bb_action_name.empty() || bb_action_name == "__default__placeholder__")
271-
{
272-
if(params.default_port_value.empty())
273-
{
274-
throw std::logic_error("Both [action_name] in the InputPort and the "
275-
"RosNodeParams are empty.");
276-
}
277-
else
278-
{
279-
createClient(params.default_port_value);
280-
}
281-
}
282-
else if(!isBlackboardPointer(bb_action_name))
272+
if(isBlackboardPointer(bb_service_name))
283273
{
284-
// If the content of the port "action_name" is not
285-
// a pointer to the blackboard, but a static string, we can
286-
// create the client in the constructor.
287-
createClient(bb_action_name);
274+
// unknown value at construction time. Postpone to tick
275+
action_name_should_be_checked_ = true;
288276
}
289-
else
277+
else if(!bb_service_name.empty())
290278
{
291-
action_name_may_change_ = true;
292-
// createClient will be invoked in the first tick().
279+
// "hard-coded" name in the bb_service_name. Use it.
280+
createClient(bb_service_name);
293281
}
294282
}
295-
else
283+
// no port value or it is empty. Use the default value
284+
if(!client_instance_ && !params.default_port_value.empty())
296285
{
297-
if(params.default_port_value.empty())
298-
{
299-
throw std::logic_error("Both [action_name] in the InputPort and the RosNodeParams "
300-
"are empty.");
301-
}
302-
else
303-
{
304-
createClient(params.default_port_value);
305-
}
286+
createClient(params.default_port_value);
306287
}
307288
}
308289

@@ -347,6 +328,13 @@ inline bool RosActionNode<T>::createClient(const std::string& action_name)
347328
return found;
348329
}
349330

331+
template <class T>
332+
inline void RosActionNode<T>::setActionName(const std::string& action_name)
333+
{
334+
action_name_ = action_name;
335+
createClient(action_name);
336+
}
337+
350338
template <class T>
351339
inline NodeStatus RosActionNode<T>::tick()
352340
{
@@ -359,7 +347,8 @@ inline NodeStatus RosActionNode<T>::tick()
359347
// First, check if the action_client_ is valid and that the name of the
360348
// action_name in the port didn't change.
361349
// otherwise, create a new client
362-
if(!client_instance_ || (status() == NodeStatus::IDLE && action_name_may_change_))
350+
if(!client_instance_ ||
351+
(status() == NodeStatus::IDLE && action_name_should_be_checked_))
363352
{
364353
std::string action_name;
365354
getInput("action_name", action_name);
@@ -368,6 +357,13 @@ inline NodeStatus RosActionNode<T>::tick()
368357
createClient(action_name);
369358
}
370359
}
360+
361+
if(!client_instance_)
362+
{
363+
throw BT::RuntimeError("RosActionNode: no client was specified neither as default or "
364+
"in the ports");
365+
}
366+
371367
auto& action_client = client_instance_->action_client;
372368

373369
//------------------------------------------

‎behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp‎

Lines changed: 30 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -96,8 +96,7 @@ class RosServiceNode : public BT::ActionNodeBase
9696
*/
9797
static PortsList providedBasicPorts(PortsList addition)
9898
{
99-
PortsList basic = { InputPort<std::string>("service_name", "__default__placeholder__",
100-
"Service name") };
99+
PortsList basic = { InputPort<std::string>("service_name", "", "Service name") };
101100
basic.insert(addition.begin(), addition.end());
102101
return basic;
103102
}
@@ -111,7 +110,7 @@ class RosServiceNode : public BT::ActionNodeBase
111110
return providedBasicPorts({});
112111
}
113112

114-
NodeStatus tick() override final;
113+
NodeStatus tick() override;
115114

116115
/// The default halt() implementation.
117116
void halt() override;
@@ -157,6 +156,9 @@ class RosServiceNode : public BT::ActionNodeBase
157156
return action_client_mutex;
158157
}
159158

159+
// method to set the service name programmatically
160+
void setServiceName(const std::string& service_name);
161+
160162
rclcpp::Logger logger()
161163
{
162164
if(auto node = node_.lock())
@@ -186,7 +188,7 @@ class RosServiceNode : public BT::ActionNodeBase
186188

187189
std::weak_ptr<rclcpp::Node> node_;
188190
std::string service_name_;
189-
bool service_name_may_change_ = false;
191+
bool service_name_should_be_checked_ = false;
190192
const std::chrono::milliseconds service_timeout_;
191193
const std::chrono::milliseconds wait_for_service_timeout_;
192194

@@ -233,51 +235,30 @@ inline RosServiceNode<T>::RosServiceNode(const std::string& instance_name,
233235
{
234236
const std::string& bb_service_name = portIt->second;
235237

236-
if(bb_service_name.empty() || bb_service_name == "__default__placeholder__")
238+
if(isBlackboardPointer(bb_service_name))
237239
{
238-
if(params.default_port_value.empty())
239-
{
240-
throw std::logic_error("Both [service_name] in the InputPort and the "
241-
"RosNodeParams are empty.");
242-
}
243-
else
244-
{
245-
createClient(params.default_port_value);
246-
}
240+
// unknown value at construction time. postpone to tick
241+
service_name_should_be_checked_ = true;
247242
}
248-
else if(!isBlackboardPointer(bb_service_name))
243+
else if(!bb_service_name.empty())
249244
{
250-
// If the content of the port "service_name" is not
251-
// a pointer to the blackboard, but a static string, we can
252-
// create the client in the constructor.
245+
// "hard-coded" name in the bb_service_name. Use it.
253246
createClient(bb_service_name);
254247
}
255-
else
256-
{
257-
service_name_may_change_ = true;
258-
// createClient will be invoked in the first tick().
259-
}
260248
}
261-
else
249+
// no port value or it is empty. Use the default port value
250+
if(!srv_instance_ && !params.default_port_value.empty())
262251
{
263-
if(params.default_port_value.empty())
264-
{
265-
throw std::logic_error("Both [service_name] in the InputPort and the RosNodeParams "
266-
"are empty.");
267-
}
268-
else
269-
{
270-
createClient(params.default_port_value);
271-
}
252+
createClient(params.default_port_value);
272253
}
273254
}
274255

275256
template <class T>
276257
inline bool RosServiceNode<T>::createClient(const std::string& service_name)
277258
{
278-
if(service_name.empty())
259+
if(service_name.empty() || service_name == "__default__placeholder__")
279260
{
280-
throw RuntimeError("service_name is empty");
261+
throw RuntimeError("service_name is empty or invalid");
281262
}
282263

283264
std::unique_lock lk(getMutex());
@@ -314,6 +295,13 @@ inline bool RosServiceNode<T>::createClient(const std::string& service_name)
314295
return found;
315296
}
316297

298+
template <class T>
299+
inline void RosServiceNode<T>::setServiceName(const std::string& service_name)
300+
{
301+
service_name_ = service_name;
302+
createClient(service_name);
303+
}
304+
317305
template <class T>
318306
inline NodeStatus RosServiceNode<T>::tick()
319307
{
@@ -326,7 +314,7 @@ inline NodeStatus RosServiceNode<T>::tick()
326314
// First, check if the service_client is valid and that the name of the
327315
// service_name in the port didn't change.
328316
// otherwise, create a new client
329-
if(!srv_instance_ || (status() == NodeStatus::IDLE && service_name_may_change_))
317+
if(!srv_instance_ || (status() == NodeStatus::IDLE && service_name_should_be_checked_))
330318
{
331319
std::string service_name;
332320
getInput("service_name", service_name);
@@ -336,6 +324,12 @@ inline NodeStatus RosServiceNode<T>::tick()
336324
}
337325
}
338326

327+
if(!srv_instance_)
328+
{
329+
throw BT::RuntimeError("RosServiceNode: no service client was specified neither as "
330+
"default or in the ports");
331+
}
332+
339333
auto CheckStatus = [](NodeStatus status) {
340334
if(!isStatusCompleted(status))
341335
{

‎behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,13 @@ struct RosNodeParams
4040
std::chrono::milliseconds server_timeout = std::chrono::milliseconds(1000);
4141
// timeout used when detecting the server the first time
4242
std::chrono::milliseconds wait_for_server_timeout = std::chrono::milliseconds(500);
43+
44+
RosNodeParams() = default;
45+
RosNodeParams(std::shared_ptr<rclcpp::Node> node) : nh(node)
46+
{}
47+
RosNodeParams(std::shared_ptr<rclcpp::Node> node, const std::string& port_name)
48+
: nh(node), default_port_value(port_name)
49+
{}
4350
};
4451

4552
} // namespace BT

‎btcpp_ros2_samples/src/bool_client.cpp‎

Lines changed: 15 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,11 @@ static const char* xml_text = R"(
1010
<root BTCPP_format="4">
1111
<BehaviorTree>
1212
<Sequence>
13-
<RobotSetBool robot="robotA" command="true"/>
14-
<RobotSetBool robot="robotB" command="true"/>
13+
14+
<SetBoolA value="true"/>
15+
<SetBool service_name="robotB/set_bool" value="true"/>
16+
<SetRobotBool robot="robotA" value="true"/>
17+
<SetRobotBool robot="robotB" value="true"/>
1518
</Sequence>
1619
</BehaviorTree>
1720
</root>
@@ -23,7 +26,16 @@ int main(int argc, char** argv)
2326
auto nh = std::make_shared<rclcpp::Node>("bool_client");
2427

2528
BehaviorTreeFactory factory;
26-
factory.registerNodeType<NamespacedSetBool>("RobotSetBool", "set_bool", nh);
29+
30+
// version with default port
31+
factory.registerNodeType<SetBoolService>("SetBoolA", BT::RosNodeParams(nh, "robotA/"
32+
"set_bool"));
33+
34+
// version without default port
35+
factory.registerNodeType<SetBoolService>("SetBool", BT::RosNodeParams(nh));
36+
37+
// namespace version
38+
factory.registerNodeType<SetRobotBoolService>("SetRobotBool", nh, "set_bool");
2739

2840
auto tree = factory.createTreeFromText(xml_text);
2941

‎btcpp_ros2_samples/src/set_bool_node.cpp‎

Lines changed: 7 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -12,12 +12,12 @@ BT::NodeStatus SetBoolService::onResponseReceived(const Response::SharedPtr& res
1212
std::cout << "onResponseReceived " << std::endl;
1313
if(response->success)
1414
{
15-
RCLCPP_INFO(logger(), "SetString service succeeded.");
15+
RCLCPP_INFO(logger(), "SetBool service succeeded.");
1616
return BT::NodeStatus::SUCCESS;
1717
}
1818
else
1919
{
20-
RCLCPP_INFO(logger(), "SetString service failed: %s", response->message.c_str());
20+
RCLCPP_INFO(logger(), "SetBool service failed: %s", response->message.c_str());
2121
return BT::NodeStatus::FAILURE;
2222
}
2323
}
@@ -28,54 +28,14 @@ BT::NodeStatus SetBoolService::onFailure(BT::ServiceNodeErrorCode error)
2828
return BT::NodeStatus::FAILURE;
2929
}
3030

31-
//------------------------------------------------------
32-
//------------------------------------------------------
33-
//------------------------------------------------------
31+
//-----------------------------------------------------------
3432

35-
NamespacedSetBool::NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf,
36-
const BT::RosNodeParams& params)
37-
: BT::ActionNodeBase(name, conf)
38-
, local_bb_(BT::Blackboard::create(conf.blackboard))
39-
, service_name_(params.default_port_value)
40-
{
41-
BT::NodeConfig impl_config;
42-
impl_config.blackboard = local_bb_;
43-
impl_config.input_ports["service_name"] = "{=}";
44-
impl_config.input_ports["value"] = "{=}";
45-
46-
BT::RosNodeParams impl_params = params;
47-
impl_params.default_port_value = {}; // postpone this
48-
set_bool_service_ = std::make_unique<SetBoolService>(name, impl_config, impl_params);
49-
}
50-
51-
NamespacedSetBool::NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf,
52-
const std::string& service_name,
53-
rclcpp::Node::SharedPtr node)
54-
: BT::ActionNodeBase(name, conf)
55-
, local_bb_(BT::Blackboard::create(conf.blackboard))
56-
, service_name_(service_name)
57-
{
58-
BT::NodeConfig impl_config;
59-
impl_config.blackboard = local_bb_;
60-
impl_config.input_ports["service_name"] = "{=}";
61-
impl_config.input_ports["value"] = "{=}";
62-
63-
BT::RosNodeParams impl_params;
64-
impl_params.nh = node;
65-
set_bool_service_ = std::make_unique<SetBoolService>(name, impl_config, impl_params);
66-
}
67-
68-
BT::NodeStatus NamespacedSetBool::tick()
33+
BT::NodeStatus SetRobotBoolService::tick()
6934
{
7035
std::string robot;
71-
bool command;
72-
if(!getInput("robot", robot) || !getInput("command", command))
36+
if(getInput("robot", robot) && !robot.empty())
7337
{
74-
throw BT::RuntimeError("NamespacedSetBool: Missing inputs");
38+
setServiceName(robot + "/" + service_suffix_);
7539
}
76-
77-
local_bb_->set("service_name", robot + "/" + service_name_);
78-
local_bb_->set("value", command);
79-
std::cout << "ticking " << std::endl;
80-
return set_bool_service_->tick();
40+
return SetBoolService::tick();
8141
}

0 commit comments

Comments
 (0)