Skip to content

Commit 4dcd00f

Browse files
daf@blue-ocean-robotics.comdaf@blue-ocean-robotics.com
[email protected]
authored and
[email protected]
committed
Full refactoring
1 parent 2e66e54 commit 4dcd00f

File tree

12 files changed

+672
-134
lines changed

12 files changed

+672
-134
lines changed

‎CMakeLists.txt

Lines changed: 28 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,19 +1,18 @@
1-
cmake_minimum_required(VERSION 2.8.12) # version on Ubuntu Trusty
1+
cmake_minimum_required(VERSION 3.5.1) # version on Ubuntu Trusty
22
project(behaviortree_ros)
33

4-
if(NOT CMAKE_VERSION VERSION_LESS 3.1)
5-
set(CMAKE_CXX_STANDARD 11)
6-
set(CMAKE_CXX_STANDARD_REQUIRED ON)
7-
else()
8-
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
9-
endif()
10-
4+
set(CMAKE_CXX_STANDARD 14)
5+
set(CMAKE_CXX_STANDARD_REQUIRED ON)
116
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
127

138
######################################################
149

1510
set(ROS_DEPENDENCIES
16-
roscpp std_msgs behaviortree_cpp_v3 actionlib_msgs actionlib message_generation)
11+
roscpp std_msgs
12+
behaviortree_cpp_v3
13+
actionlib_msgs
14+
actionlib
15+
message_generation)
1716

1817
find_package(catkin REQUIRED COMPONENTS ${ROS_DEPENDENCIES} )
1918
find_package(GTest)
@@ -28,9 +27,20 @@ add_message_files(
2827
TreeNode.msg
2928
)
3029

30+
add_service_files(
31+
FILES
32+
AddTwoInts.srv
33+
)
34+
35+
add_action_files(
36+
DIRECTORY action
37+
FILES Fibonacci.action
38+
)
39+
3140
generate_messages(
3241
DEPENDENCIES
3342
std_msgs
43+
actionlib_msgs
3444
)
3545

3646
catkin_package(
@@ -47,15 +57,22 @@ include_directories( include ${catkin_INCLUDE_DIRS})
4757

4858
add_library(behaviortree_ros
4959
src/loggers/rosout_logger.cpp
50-
src/actions/movebase_client.cpp
60+
src/bt_service_node.cpp
5161
)
5262

53-
target_link_libraries(behaviortree_ros ${catkin_LIBRARIES})
63+
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
5464

5565

5666
######################################################
5767
# TESTS
5868

69+
add_executable(test_bt test/test_bt.cpp)
70+
add_dependencies(test_bt ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
71+
target_link_libraries(test_bt ${catkin_LIBRARIES} ${PROJECT_NAME})
72+
73+
add_executable(test_server test/test_server.cpp)
74+
add_dependencies(test_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
75+
target_link_libraries(test_server ${catkin_LIBRARIES} ${PROJECT_NAME})
5976

6077
######################################################
6178
# INSTALL

‎action/Fibonacci.action

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
#goal definition
2+
int32 order
3+
---
4+
#result definition
5+
int32[] sequence
6+
---
7+
#feedback
8+
int32[] sequence

‎include/behaviortree_ros/bt_action_node.h

Lines changed: 173 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,173 @@
1+
// Copyright (c) 2019 Samsung Research America
2+
// Copyright (c) 2020 Davide Faconti
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#ifndef BEHAVIOR_TREE_BT_ACTION_NODE_HPP_
17+
#define BEHAVIOR_TREE_BT_ACTION_NODE_HPP_
18+
19+
#include <behaviortree_cpp_v3/action_node.h>
20+
#include <behaviortree_cpp_v3/bt_factory.h>
21+
#include <ros/ros.h>
22+
#include <actionlib/client/simple_action_client.h>
23+
24+
namespace BT
25+
{
26+
27+
template<class ActionT>
28+
class RosActionNode : public BT::ActionNodeBase
29+
{
30+
protected:
31+
32+
RosActionNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration & conf):
33+
BT::ActionNodeBase(name, conf), node_(nh)
34+
{
35+
const std::string server_name = getInput<std::string>("server_name").value();
36+
action_client_ = std::make_shared<ActionClientType>( node_, server_name, true );
37+
}
38+
39+
public:
40+
41+
using BaseClass = RosActionNode<ActionT>;
42+
using ActionClientType = actionlib::SimpleActionClient<ActionT>;
43+
using ActionType = ActionT;
44+
using GoalType = typename ActionT::_action_goal_type::_goal_type;
45+
using ResultType = typename ActionT::_action_result_type::_result_type;
46+
47+
RosActionNode() = delete;
48+
49+
virtual ~RosActionNode() = default;
50+
51+
static PortsList providedPorts()
52+
{
53+
return {
54+
InputPort<std::string>("server_name", "name of the Action Server"),
55+
InputPort<unsigned>("timeout", 500, "timeout to connect (milliseconds)")
56+
};
57+
}
58+
59+
virtual bool sendGoal(GoalType& goal) = 0;
60+
61+
/// Method (to be implemented by the user) to receive the reply.
62+
/// User can decide which NodeStatus it will return (SUCCESS or FAILURE).
63+
virtual NodeStatus onResult( const ResultType& res) = 0;
64+
65+
66+
enum FailureCause{
67+
MISSING_SERVER = 0,
68+
ABORTED_BY_SERVER = 1,
69+
REJECTED_BY_SERVER = 2
70+
};
71+
72+
/// Called when a service call failed. Can be overriden by the user.
73+
virtual NodeStatus onFailedRequest(FailureCause failure)
74+
{
75+
return NodeStatus::FAILURE;
76+
}
77+
78+
virtual void halt() override
79+
{
80+
if( status() == NodeStatus::RUNNING )
81+
{
82+
action_client_->cancelGoal();
83+
}
84+
setStatus(NodeStatus::IDLE);
85+
}
86+
87+
protected:
88+
89+
std::shared_ptr<ActionClientType> action_client_;
90+
91+
ros::NodeHandle& node_;
92+
93+
BT::NodeStatus tick() override
94+
{
95+
96+
unsigned msec = getInput<unsigned>("timeout").value();
97+
ros::Duration timeout(static_cast<double>(msec) * 1e-3);
98+
99+
bool connected = action_client_->waitForServer(timeout);
100+
if( !connected ){
101+
return onFailedRequest(MISSING_SERVER);
102+
}
103+
104+
// first step to be done only at the beginning of the Action
105+
if (status() == BT::NodeStatus::IDLE) {
106+
// setting the status to RUNNING to notify the BT Loggers (if any)
107+
setStatus(BT::NodeStatus::RUNNING);
108+
109+
GoalType goal;
110+
bool valid_goal = sendGoal(goal);
111+
if( !valid_goal )
112+
{
113+
return NodeStatus::FAILURE;
114+
}
115+
116+
action_client_->sendGoal(goal);
117+
}
118+
119+
// RUNNING
120+
auto action_state = action_client_->getState();
121+
122+
// Please refer to these states
123+
124+
if( action_state == actionlib::SimpleClientGoalState::PENDING ||
125+
action_state == actionlib::SimpleClientGoalState::ACTIVE )
126+
{
127+
return NodeStatus::RUNNING;
128+
}
129+
else if( action_state == actionlib::SimpleClientGoalState::SUCCEEDED)
130+
{
131+
return onResult( *action_client_->getResult() );
132+
}
133+
else if( action_state == actionlib::SimpleClientGoalState::ABORTED)
134+
{
135+
return onFailedRequest( ABORTED_BY_SERVER );
136+
}
137+
else if( action_state == actionlib::SimpleClientGoalState::REJECTED)
138+
{
139+
return onFailedRequest( REJECTED_BY_SERVER );
140+
}
141+
else
142+
{
143+
// FIXME: is there any other valid state we should consider?
144+
throw std::logic_error("Unexpected state in RosActionNode::tick()");
145+
}
146+
}
147+
};
148+
149+
150+
/// Method to register the service into a factory.
151+
/// It gives you the opportunity to set the ros::NodeHandle.
152+
template <class DerivedT> static
153+
void RegisterRosAction(BT::BehaviorTreeFactory& factory,
154+
const std::string& registration_ID,
155+
ros::NodeHandle& node_handle)
156+
{
157+
NodeBuilder builder = [&node_handle](const std::string& name, const NodeConfiguration& config) {
158+
return std::make_unique<DerivedT>(node_handle, name, config );
159+
};
160+
161+
TreeNodeManifest manifest;
162+
manifest.type = getType<DerivedT>();
163+
manifest.ports = DerivedT::providedPorts();
164+
manifest.registration_ID = registration_ID;
165+
const auto& basic_ports = RosActionNode< typename DerivedT::ActionType>::providedPorts();
166+
manifest.ports.insert( basic_ports.begin(), basic_ports.end() );
167+
factory.registerBuilder( manifest, builder );
168+
}
169+
170+
171+
} // namespace BT
172+
173+
#endif // BEHAVIOR_TREE_BT_ACTION_NODE_HPP_

‎include/behaviortree_ros/bt_service_node.h

Lines changed: 132 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,132 @@
1+
// Copyright (c) 2019 Samsung Research America
2+
// Copyright (c) 2020 Davide Faconti
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#ifndef BEHAVIOR_TREE_BT_SERVICE_NODE_HPP_
17+
#define BEHAVIOR_TREE_BT_SERVICE_NODE_HPP_
18+
19+
#include <behaviortree_cpp_v3/action_node.h>
20+
#include <behaviortree_cpp_v3/bt_factory.h>
21+
#include <ros/ros.h>
22+
#include <ros/service_client.h>
23+
24+
namespace BT
25+
{
26+
27+
template<class ServiceT>
28+
class RosServiceNode : public BT::SyncActionNode
29+
{
30+
protected:
31+
32+
RosServiceNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration & conf):
33+
BT::SyncActionNode(name, conf), node_(nh) { }
34+
35+
public:
36+
37+
using ServiceType = ServiceT;
38+
using RequestType = typename ServiceT::Request;
39+
using ResponseType = typename ServiceT::Response;
40+
41+
RosServiceNode() = delete;
42+
43+
virtual ~RosServiceNode() = default;
44+
45+
static PortsList providedPorts()
46+
{
47+
return {
48+
InputPort<std::string>("service_name", "name of the ROS service"),
49+
InputPort<unsigned>("timeout", 100, "timeout to connect (milliseconds)")
50+
};
51+
}
52+
53+
/// User must implement this method
54+
virtual void sendRequest(RequestType& request) = 0;
55+
56+
/// Method (to be implemented by the user) to receive the reply.
57+
/// User can decide which NodeStatus it will return (SUCCESS or FAILURE).
58+
virtual NodeStatus onResponse( const ResponseType& rep) = 0;
59+
60+
enum FailureCause{
61+
MISSING_SERVER = 0,
62+
FAILED_CALL = 1
63+
};
64+
65+
/// Called when a service call failed. Can be overriden by the user.
66+
virtual NodeStatus onFailedRequest(FailureCause failure)
67+
{
68+
return NodeStatus::FAILURE;
69+
}
70+
71+
protected:
72+
73+
ros::ServiceClient service_client_;
74+
75+
typename ServiceT::Response reply_;
76+
77+
// The node that will be used for any ROS operations
78+
ros::NodeHandle& node_;
79+
80+
BT::NodeStatus tick() override
81+
{
82+
if( !service_client_.isValid() ){
83+
std::string server = getInput<std::string>("service_name").value();
84+
service_client_ = node_.serviceClient<ServiceT>( server );
85+
}
86+
87+
unsigned msec;
88+
getInput("timeout", msec);
89+
ros::Duration timeout(static_cast<double>(msec) * 1e-3);
90+
91+
bool connected = service_client_.waitForExistence(timeout);
92+
if( !connected ){
93+
return onFailedRequest(MISSING_SERVER);
94+
}
95+
96+
typename ServiceT::Request request;
97+
sendRequest(request);
98+
bool received = service_client_.call( request, reply_ );
99+
if( !received )
100+
{
101+
return onFailedRequest(FAILED_CALL);
102+
}
103+
return onResponse(reply_);
104+
}
105+
};
106+
107+
108+
/// Method to register the service into a factory.
109+
/// It gives you the opportunity to set the ros::NodeHandle.
110+
template <class DerivedT> static
111+
void RegisterRosService(BT::BehaviorTreeFactory& factory,
112+
const std::string& registration_ID,
113+
ros::NodeHandle& node_handle)
114+
{
115+
NodeBuilder builder = [&node_handle](const std::string& name, const NodeConfiguration& config) {
116+
return std::make_unique<DerivedT>(node_handle, name, config );
117+
};
118+
119+
TreeNodeManifest manifest;
120+
manifest.type = getType<DerivedT>();
121+
manifest.ports = DerivedT::providedPorts();
122+
manifest.registration_ID = registration_ID;
123+
const auto& basic_ports = RosServiceNode< typename DerivedT::ServiceType>::providedPorts();
124+
manifest.ports.insert( basic_ports.begin(), basic_ports.end() );
125+
126+
factory.registerBuilder( manifest, builder );
127+
}
128+
129+
130+
} // namespace BT
131+
132+
#endif // BEHAVIOR_TREE_BT_SERVICE_NODE_HPP_

‎include/behaviortree_ros/loggers/rosout_logger.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
#ifndef BT_ROSOUT_LOGGER_H
22
#define BT_ROSOUT_LOGGER_H
33

4-
#include <behaviortree_cpp/loggers/abstract_logger.h>
4+
#include <behaviortree_cpp_v3/loggers/abstract_logger.h>
55
#include <ros/console.h>
66

77
namespace BT

0 commit comments

Comments
 (0)