forked from BehaviorTree/BehaviorTree.ROS
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathtest_server.cpp
More file actions
117 lines (95 loc) · 3.02 KB
/
test_server.cpp
File metadata and controls
117 lines (95 loc) · 3.02 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <behaviortree_ros/AddTwoInts.h>
#include <behaviortree_ros/FibonacciAction.h>
bool Add(behaviortree_ros::AddTwoInts::Request &req,
behaviortree_ros::AddTwoInts::Response &res)
{
res.sum = req.a + req.b;
ROS_INFO("request: x=%d, y=%d", req.a, req.b);
ROS_INFO("sending back response: [%d]", res.sum);
return true;
}
// Code inspeired by
// http://wiki.ros.org/actionlib_tutorials/Tutorials/SimpleActionServer%28ExecuteCallbackMethod%29
class FibonacciServer
{
protected:
ros::NodeHandle nh_;
// NodeHandle instance must be created before this line. Otherwise strange error occurs.
actionlib::SimpleActionServer<behaviortree_ros::FibonacciAction> server_;
std::string action_name_;
// create messages that are used to published feedback/result
behaviortree_ros::FibonacciFeedback feedback_;
behaviortree_ros::FibonacciResult result_;
int call_number_;
public:
FibonacciServer(std::string name) :
server_(nh_, name, boost::bind(&FibonacciServer::executeCB, this, _1), false),
action_name_(name)
{
server_.start();
call_number_ = 0;
}
~FibonacciServer(void)
{
}
void executeCB(const behaviortree_ros::FibonacciGoalConstPtr &goal)
{
// calculate the result
// push_back the seeds for the fibonacci sequence
feedback_.sequence.clear();
feedback_.sequence.push_back(0);
feedback_.sequence.push_back(1);
// publish info to the console for the user
ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i",
action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
for(int i=1; i<=goal->order; i++)
{
feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
server_.publishFeedback(feedback_);
}
bool preempted = false;
// simulate a long period of time.
// check that preempt has not been requested by the client
int required_time = 500 ;// 0.5 sec;
if( call_number_ % 2 == 0)
{
required_time = 100;
}
// check periodically for preemption
while ( required_time > 0 )
{
if (server_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
server_.setPreempted();
preempted = true;
break;
}
ros::Duration take_break(0.010);
take_break.sleep();
required_time -= 10;
}
if(!preempted)
{
result_.sequence = feedback_.sequence;
ROS_INFO("%s: Succeeded", action_name_.c_str());
server_.setSucceeded(result_);
}
else{
ROS_WARN("%s: Preempted", action_name_.c_str());
}
}
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "add_two_ints_server");
ros::NodeHandle n;
ros::ServiceServer service = n.advertiseService("add_two_ints", Add);
ROS_INFO("Ready to add two ints.");
FibonacciServer fibonacci("fibonacci");
ros::spin();
return 0;
}