test/planner_action.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include <gmock/gmock.h>
3 
7 #include <mbf_msgs/GetPathAction.h>
8 
11 
12 #include <boost/chrono.hpp>
13 #include <boost/thread/thread.hpp>
14 #include <boost/thread/condition.hpp>
15 
16 #include <string>
17 
18 using namespace mbf_abstract_nav;
19 
20 // in this test we will use an action server and an action client, since
21 // we cannot mock the goal-handle.
22 
23 using geometry_msgs::PoseStamped;
25 
26 // a mocked planner, allowing to control the outcome
28 {
29  MOCK_METHOD6(makePlan, uint32_t(const PoseStamped&, const PoseStamped&, double, std::vector<PoseStamped>&, double&,
30  std::string&));
31 
32  // the cancel call is for now uninteresting. if this changes, we can also mock it.
33  bool cancel()
34  {
35  return true;
36  }
37 };
38 
40 using mbf_msgs::GetPathAction;
41 
42 using testing::_;
43 using testing::DoAll;
44 using testing::Eq;
45 using testing::Field;
46 using testing::Return;
47 using testing::SetArgReferee;
48 using testing::Test;
49 
50 // a mocked action server
51 struct MockedActionServer : public actionlib::ActionServerBase<GetPathAction>
52 {
53  // define the types we are using
56 
57  MockedActionServer(boost::function<void(GoalHandle)> goal_cb, boost::function<void(GoalHandle)> cancel_cb)
58  : ActionServerBase(goal_cb, cancel_cb, true)
59  {
60  }
61 
62  // the mocked method
63  MOCK_METHOD2(publishResult, void(const actionlib_msgs::GoalStatus&, const Result&));
64 
65  // methods below are not required for now but might be mocked in the future
66  virtual void initialize()
67  {
68  }
69 
70  void publishFeedback(const actionlib_msgs::GoalStatus&, const Feedback&)
71  {
72  }
73 
75  {
76  }
77 };
78 
79 // action which will trigger our condition variable, so we can wait until the runImpl thread reaches makePlan
80 ACTION_P(Notify, cv)
81 {
82  cv->notify_all();
83 }
84 
85 // test-fixture
86 struct PlannerActionFixture : public Test
87 {
89  boost::condition_variable done_condition_;
90  MoveBaseFlexConfig config;
91 
92  std::string action_name;
94 
95  // todo change this from const ref
96  std::string global_frame;
97  std::string local_frame;
100 
103 
105 
106  mbf_msgs::GetPathResult expected_result;
107  mbf_msgs::GetPathActionGoalPtr goal;
108 
110  : planner(new MockPlanner())
111  , action_name("action_name")
112  , tf(new TF())
113  , global_frame("global_frame")
114  , local_frame("local_frame")
115  , dur(0)
116  , goal(new mbf_msgs::GetPathActionGoal())
117  , robot_info(*tf, global_frame, local_frame, dur)
118  , planner_action(action_name, robot_info)
119  , action_server(boost::bind(&PlannerActionFixture::callAction, this, _1),
120  boost::bind(&PlannerActionFixture::cancelAction, this, _1))
121  {
122  tf->setUsingDedicatedThread(true);
123  config.planner_patience = 0;
124  }
125 
126  void TearDown()
127  {
128  // call the server - this is where the actual test call happens
129  action_server.goalCallback(goal);
130 
131  // wait until the cv gets triggered
132  boost::mutex m;
133  boost::unique_lock<boost::mutex> lock(m);
134  done_condition_.wait_for(lock, boost::chrono::seconds(1));
135  }
136 
138  {
139  planner_action.start(goal_handle, boost::make_shared<AbstractPlannerExecution>("plugin", planner, tf, config));
140  }
141 
143  {
144  planner_action.cancel(goal_handle);
145  }
146 };
147 
149 {
150  // goal with frames, so we can pass tf-lookup
151  goal->goal.use_start_pose = true;
152  goal->goal.start_pose.header.frame_id = goal->goal.target_pose.header.frame_id = global_frame;
153 
154  // setup the expectation
155  // we dont return anything here, so the outcome should be empty path
156  EXPECT_CALL(*planner, makePlan(_, _, _, _, _, _)).WillOnce(Return(0));
157  EXPECT_CALL(action_server,
158  publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::EMPTY_PATH))))
159  .Times(1)
160  .WillOnce(Notify(&done_condition_));
161 }
162 
163 #if !ROS_VERSION_MINIMUM(1, 14, 0)
164 // disable the test on kinetic and lunar, until I figure out how to transform
165 // from global_frame into global_frame in the tf framework
166 TEST_F(PlannerActionFixture, DISABLED_success)
167 #else
169 #endif
170 {
171  // create a dummy path
172  std::vector<geometry_msgs::PoseStamped> path(10);
173  // set the frame such that we can skip tf
174  for (size_t ii = 0; ii != path.size(); ++ii)
175  {
176  path[ii].header.frame_id = global_frame;
177  path[ii].pose.orientation.w = 1;
178  }
179 
180  // goal with frames, so we can pass tf-lookup
181  goal->goal.use_start_pose = true;
182  goal->goal.start_pose.header.frame_id = goal->goal.target_pose.header.frame_id = global_frame;
183 
184  // setup the expectation
185  EXPECT_CALL(*planner, makePlan(_, _, _, _, _, _)).WillOnce(DoAll(SetArgReferee<3>(path), Return(0)));
186  EXPECT_CALL(action_server,
187  publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::SUCCESS))))
188  .Times(1)
189  .WillOnce(Notify(&done_condition_));
190 }
191 
193 {
194  // create a dummy path
195  std::vector<geometry_msgs::PoseStamped> path(10);
196  // set the frame such that we fail at the tf
197  for (size_t ii = 0; ii != path.size(); ++ii)
198  path[ii].header.frame_id = "unknown";
199 
200  // setup the expectation - we succeed here
201  EXPECT_CALL(*planner, makePlan(_, _, _, _, _, _)).WillOnce(DoAll(SetArgReferee<3>(path), Return(0)));
202  EXPECT_CALL(action_server,
203  publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::TF_ERROR))))
204  .Times(1)
205  .WillOnce(Notify(&done_condition_));
206 
207  // goal with frames, so we can pass tf-lookup
208  goal->goal.use_start_pose = true;
209  goal->goal.start_pose.header.frame_id = goal->goal.target_pose.header.frame_id = global_frame;
210 }
211 
213 {
214  // test case where we fail to get a valid robot pose.
215  // in this case we will receive a TF_ERROR from the server.
216 
217  // make us use the robot pose
218  goal->goal.use_start_pose = false;
219  goal->goal.start_pose.header.frame_id = goal->goal.target_pose.header.frame_id = global_frame;
220 
221  // set the expectation
222  EXPECT_CALL(action_server,
223  publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::TF_ERROR))))
224  .Times(1)
225  .WillOnce(Notify(&done_condition_));
226 }
227 
229 {
230  // test case where the planner fails.
231  // in this case we will receive NO_PLAN_FOUND from the server.
232  config.planner_max_retries = 0;
233 
234  // valid goal
235  goal->goal.use_start_pose = true;
236  goal->goal.start_pose.header.frame_id = goal->goal.target_pose.header.frame_id = global_frame;
237 
238  // set the expectation: the planner retuns a failure
239  EXPECT_CALL(*planner, makePlan(_, _, _, _, _, _)).WillOnce(Return(mbf_msgs::GetPathResult::NO_PATH_FOUND));
240  EXPECT_CALL(action_server,
241  publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::NO_PATH_FOUND))))
242  .Times(1)
243  .WillOnce(Notify(&done_condition_));
244 }
245 
246 ACTION(SleepAndFail)
247 {
248  boost::this_thread::sleep_for(boost::chrono::milliseconds(10));
249  return 11;
250 }
251 
253 {
254  // test case where the planner fails multple times and we are out of patience
255 
256  // setup the config; this will be passed to the execution
257  config.planner_max_retries = 5;
258  config.planner_patience = 0.001;
259 
260  // valid goal
261  goal->goal.use_start_pose = true;
262  goal->goal.start_pose.header.frame_id = goal->goal.target_pose.header.frame_id = global_frame;
263 
264  // set the expectation: the planner retuns a failure
265  EXPECT_CALL(*planner, makePlan(_, _, _, _, _, _)).WillRepeatedly(SleepAndFail());
266  EXPECT_CALL(action_server,
267  publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::PAT_EXCEEDED))))
268  .Times(1)
269  .WillOnce(Notify(&done_condition_));
270 }
271 
272 int main(int argc, char** argv)
273 {
274  ros::init(argc, argv, "read_types");
275  ros::NodeHandle nh;
276  testing::InitGoogleTest(&argc, argv);
277  return RUN_ALL_TESTS();
278 }
PlannerActionFixture::config
MoveBaseFlexConfig config
config for the mbf
Definition: test/planner_action.cpp:90
MockedActionServer::initialize
virtual void initialize()
Definition: test/planner_action.cpp:66
PlannerActionFixture::callAction
void callAction(MockedActionServer::GoalHandle goal_handle)
Definition: test/planner_action.cpp:137
PlannerActionFixture::PlannerActionFixture
PlannerActionFixture()
Definition: test/planner_action.cpp:109
PlannerActionFixture::global_frame
std::string global_frame
Definition: test/planner_action.cpp:96
boost::shared_ptr< MockPlanner >
actionlib::ActionServerBase
actionlib::ServerGoalHandle
MockedActionServer::GoalHandle
actionlib::ServerGoalHandle< GetPathAction > GoalHandle
Definition: test/planner_action.cpp:54
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
mbf_utility::RobotInformation
main
int main(int argc, char **argv)
Definition: test/planner_action.cpp:272
PlannerActionFixture::action_server
MockedActionServer action_server
Definition: test/planner_action.cpp:104
MockedActionServer::ActionServerBase
actionlib::ActionServerBase< GetPathAction > ActionServerBase
Definition: test/planner_action.cpp:55
actionlib::ActionServerBase::goalCallback
void goalCallback(const boost::shared_ptr< const ActionGoal > &goal)
PlannerActionFixture
Definition: test/planner_action.cpp:86
simple_action_server.h
boost
MockPlanner::cancel
bool cancel()
Definition: test/planner_action.cpp:33
mbf_abstract_nav
Definition: abstract_controller_execution.h:58
planner
EXPECT_CALL * planner(_, _, _, _, _, _)).WillOnce(DoAll(SetArgReferee< 3 >(path), Return(0))
PlannerActionFixture::done_condition_
boost::condition_variable done_condition_
cv triggered on makePlan
Definition: test/planner_action.cpp:89
TEST_F
TEST_F(PlannerActionFixture, emptyPath)
Definition: test/planner_action.cpp:148
PlannerActionFixture::action_name
std::string action_name
Definition: test/planner_action.cpp:92
MockedActionServer::MockedActionServer
MockedActionServer(boost::function< void(GoalHandle)> goal_cb, boost::function< void(GoalHandle)> cancel_cb)
Definition: test/planner_action.cpp:57
PlannerActionFixture::TearDown
void TearDown()
Definition: test/planner_action.cpp:126
simple_action_client.h
PlannerActionFixture::tf
TFPtr tf
Definition: test/planner_action.cpp:93
MockedActionServer::publishFeedback
void publishFeedback(const actionlib_msgs::GoalStatus &, const Feedback &)
Definition: test/planner_action.cpp:70
ACTION
ACTION(SleepAndFail)
Definition: test/planner_action.cpp:246
PlannerActionFixture::cancelAction
void cancelAction(MockedActionServer::GoalHandle goal_handle)
Definition: test/planner_action.cpp:142
PlannerActionFixture::dur
ros::Duration dur
Definition: test/planner_action.cpp:98
PlannerActionFixture::nh
ros::NodeHandle nh
Definition: test/planner_action.cpp:102
abstract_planner.h
EXPECT_CALL
EXPECT_CALL(action_server, publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::SUCCESS)))) .Times(1) .WillOnce(Notify(&done_condition_))
MockedActionServer::publishStatus
void publishStatus()
Definition: test/planner_action.cpp:74
PlannerActionFixture::local_frame
std::string local_frame
Definition: test/planner_action.cpp:97
mbf_abstract_core::AbstractPlanner
ACTION_P
ACTION_P(Notify, cv)
Definition: test/planner_action.cpp:80
PlannerActionFixture::planner_action
PlannerAction planner_action
Definition: test/planner_action.cpp:101
tf::TransformListener
MockPlanner
Definition: test/planner_action.cpp:27
PlannerActionFixture::robot_info
mbf_utility::RobotInformation robot_info
Definition: test/planner_action.cpp:99
mbf_abstract_nav::AbstractPlannerExecution
The AbstractPlannerExecution class loads and binds the global planner plugin. It contains a thread ru...
Definition: abstract_planner_execution.h:74
tf
MockedActionServer
Definition: test/planner_action.cpp:51
header
const std::string header
PlannerActionFixture::expected_result
mbf_msgs::GetPathResult expected_result
Definition: test/planner_action.cpp:106
ros::Duration
PlannerActionFixture::planner
boost::shared_ptr< MockPlanner > planner
the mocked planner
Definition: test/planner_action.cpp:88
mbf_abstract_nav::PlannerAction
Definition: planner_action.h:56
ros::NodeHandle
planner_action.h
PlannerActionFixture::goal
mbf_msgs::GetPathActionGoalPtr goal
Definition: test/planner_action.cpp:107
abstract_planner_execution.h


mbf_abstract_nav
Author(s): Sebastian Pütz
autogenerated on Wed Mar 2 2022 00:33:47