Skip to content

Commit bb5535e

Browse files
Change the way default port is passed, to allow namespaces
1 parent 3da7c29 commit bb5535e

File tree

10 files changed

+270
-73
lines changed

10 files changed

+270
-73
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

behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,15 @@ class TreeExecutionServer
7676
BT::BehaviorTreeFactory& factory();
7777

7878
protected:
79+
/**
80+
* @brief Callback invoked when a goal is received and before the tree is created.
81+
* If it returns false, the goal will be rejected.
82+
*/
83+
virtual bool onGoalReceived(const std::string& tree_name, const std::string& payload)
84+
{
85+
return true;
86+
}
87+
7988
/**
8089
* @brief Callback invoked after the tree is created.
8190
* It can be used, for instance, to initialize a logger or the global blackboard.

behaviortree_ros2/src/tree_execution_server.cpp

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -121,6 +121,11 @@ TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */,
121121
{
122122
RCLCPP_INFO(kLogger, "Received goal request to execute Behavior Tree: %s",
123123
goal->target_tree.c_str());
124+
125+
if(!onGoalReceived(goal->target_tree, goal->payload))
126+
{
127+
return rclcpp_action::GoalResponse::REJECT;
128+
}
124129
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
125130
}
126131

btcpp_ros2_samples/CMakeLists.txt

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,12 @@ find_package(ament_cmake REQUIRED)
99
find_package(behaviortree_ros2 REQUIRED)
1010
find_package(btcpp_ros2_interfaces REQUIRED)
1111
find_package(std_msgs REQUIRED)
12+
find_package(std_srvs REQUIRED)
1213

1314
set(THIS_PACKAGE_DEPS
1415
behaviortree_ros2
1516
std_msgs
17+
std_srvs
1618
btcpp_ros2_interfaces )
1719

1820
######################################################
@@ -50,6 +52,14 @@ ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS})
5052
add_executable(subscriber_test src/subscriber_test.cpp)
5153
ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS})
5254

55+
######################################################
56+
# the SetBool test
57+
add_executable(bool_client src/bool_client.cpp src/set_bool_node.cpp)
58+
ament_target_dependencies(bool_client ${THIS_PACKAGE_DEPS})
59+
60+
add_executable(bool_server src/bool_server.cpp )
61+
ament_target_dependencies(bool_server ${THIS_PACKAGE_DEPS})
62+
5363
######################################################
5464
# INSTALL
5565

@@ -60,6 +70,8 @@ install(TARGETS
6070
sleep_plugin
6171
subscriber_test
6272
sample_bt_executor
73+
bool_client
74+
bool_server
6375
DESTINATION lib/${PROJECT_NAME}
6476
)
6577

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
#include "behaviortree_ros2/bt_action_node.hpp"
2+
#include "rclcpp/rclcpp.hpp"
3+
#include "rclcpp/executors.hpp"
4+
5+
#include "set_bool_node.hpp"
6+
7+
using namespace BT;
8+
9+
static const char* xml_text = R"(
10+
<root BTCPP_format="4">
11+
<BehaviorTree>
12+
<Sequence>
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"/>
18+
</Sequence>
19+
</BehaviorTree>
20+
</root>
21+
)";
22+
23+
int main(int argc, char** argv)
24+
{
25+
rclcpp::init(argc, argv);
26+
auto nh = std::make_shared<rclcpp::Node>("bool_client");
27+
28+
BehaviorTreeFactory factory;
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");
39+
40+
auto tree = factory.createTreeFromText(xml_text);
41+
42+
tree.tickWhileRunning();
43+
44+
return 0;
45+
}

0 commit comments

Comments
 (0)