test_abort.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
31 #include <gtest/gtest.h>
32 #include <move_base_msgs/MoveBaseAction.h>
33 #include <planner_cspace_msgs/PlannerStatus.h>
34 #include <ros/ros.h>
35 #include <memory>
36 
37 class AbortTest : public ::testing::Test
38 {
39 public:
41  : node_()
42  {
43  move_base_ = std::make_shared<ActionClient>("/move_base");
44  status_sub_ = node_.subscribe("/planner_3d/status",
45  10,
47  this);
48  if (!move_base_->waitForServer(ros::Duration(30.0)))
49  {
50  ROS_ERROR("Failed to connect move_base action");
51  exit(EXIT_FAILURE);
52  }
53  }
55  {
56  }
57 
58 protected:
60  using ActionClientPtr = std::shared_ptr<ActionClient>;
61 
62  void StatusCallback(const planner_cspace_msgs::PlannerStatus& msg)
63  {
64  status_ = msg;
65  }
66  move_base_msgs::MoveBaseGoal CreateGoalInRock()
67  {
68  move_base_msgs::MoveBaseGoal goal;
69  goal.target_pose.header.stamp = ros::Time::now();
70  goal.target_pose.header.frame_id = "map";
71  goal.target_pose.pose.position.x = 1.19;
72  goal.target_pose.pose.position.y = 1.90;
73  goal.target_pose.pose.position.z = 0.0;
74  goal.target_pose.pose.orientation.x = 0.0;
75  goal.target_pose.pose.orientation.y = 0.0;
76  goal.target_pose.pose.orientation.z = 0.0;
77  goal.target_pose.pose.orientation.w = 1.0;
78  return goal;
79  }
80  move_base_msgs::MoveBaseGoal CreateGoalInFree()
81  {
82  move_base_msgs::MoveBaseGoal goal;
83  goal.target_pose.header.stamp = ros::Time::now();
84  goal.target_pose.header.frame_id = "map";
85  goal.target_pose.pose.position.x = 1.24;
86  goal.target_pose.pose.position.y = 0.65;
87  goal.target_pose.pose.position.z = 0.0;
88  goal.target_pose.pose.orientation.x = 0.0;
89  goal.target_pose.pose.orientation.y = 0.0;
90  goal.target_pose.pose.orientation.z = 0.0;
91  goal.target_pose.pose.orientation.w = 1.0;
92  return goal;
93  }
94 
98  planner_cspace_msgs::PlannerStatus status_;
99 };
100 
101 TEST_F(AbortTest, AbortByGoalInRock)
102 {
103  // Assure that goal is received after map in planner_3d.
104  ros::Duration(0.5).sleep();
105  // Send a goal which is in Rock
106  move_base_->sendGoal(CreateGoalInRock());
107  while (move_base_->getState().state_ !=
109  {
110  ros::Duration(1.0).sleep();
111  }
112 
113  // Try to replan
114  while (move_base_->getState().state_ ==
116  {
117  ros::Duration(1.0).sleep();
118  }
119  ros::Duration(1.0).sleep();
120 
121  // Abort after exceeding max_retry_num
123  move_base_->getState().state_);
124  ASSERT_EQ(planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND,
125  status_.error);
126 
127  // Send another goal which is not in Rock
128  move_base_->sendGoal(CreateGoalInFree());
129  while (move_base_->getState().state_ !=
131  {
132  ros::Duration(1.0).sleep();
133  }
134  while (move_base_->getState().state_ ==
136  {
137  ros::Duration(1.0).sleep();
138  }
139  ros::Duration(1.0).sleep();
140 
141  // Succeed
143  move_base_->getState().state_);
144  ASSERT_EQ(planner_cspace_msgs::PlannerStatus::GOING_WELL,
145  status_.error);
146 }
147 
148 int main(int argc, char** argv)
149 {
150  testing::InitGoogleTest(&argc, argv);
151  ros::init(argc, argv, "test_abort");
152 
154  spinner.start();
155  int ret = RUN_ALL_TESTS();
156  spinner.stop();
157  ros::shutdown();
158  return ret;
159 }
int main(int argc, char **argv)
Definition: test_abort.cpp:148
planner_cspace_msgs::PlannerStatus status_
Definition: test_abort.cpp:98
move_base_msgs::MoveBaseGoal CreateGoalInRock()
Definition: test_abort.cpp:66
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool sleep() const
ros::NodeHandle node_
Definition: test_abort.cpp:95
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void spinner()
ActionClientPtr move_base_
Definition: test_abort.cpp:97
TEST_F(AbortTest, AbortByGoalInRock)
Definition: test_abort.cpp:101
void StatusCallback(const planner_cspace_msgs::PlannerStatus &msg)
Definition: test_abort.cpp:62
ros::Subscriber status_sub_
Definition: test_abort.cpp:96
move_base_msgs::MoveBaseGoal CreateGoalInFree()
Definition: test_abort.cpp:80
static Time now()
ROSCPP_DECL void shutdown()
std::shared_ptr< ActionClient > ActionClientPtr
Definition: test_abort.cpp:60
#define ROS_ERROR(...)


planner_cspace
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:42