test_preempt.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 
30 #include <memory>
31 
32 #include <ros/ros.h>
34 #include <move_base_msgs/MoveBaseAction.h>
35 #include <planner_cspace_msgs/PlannerStatus.h>
36 
37 #include <gtest/gtest.h>
38 
39 class PreemptTest : public ::testing::Test
40 {
41 public:
43  : node_()
44  {
45  move_base_ = std::make_shared<ActionClient>("/move_base");
46  status_sub_ = node_.subscribe("/planner_3d/status",
47  10,
49  this);
50  if (!move_base_->waitForServer(ros::Duration(30.0)))
51  {
52  ROS_ERROR("Failed to connect move_base action");
53  exit(EXIT_FAILURE);
54  }
55  }
57  {
58  }
59 
60 protected:
62  using ActionClientPtr = std::shared_ptr<ActionClient>;
63 
64  void StatusCallback(const planner_cspace_msgs::PlannerStatus& msg)
65  {
66  status_ = msg;
67  }
68  move_base_msgs::MoveBaseGoal CreateGoalInFree()
69  {
70  move_base_msgs::MoveBaseGoal goal;
71  goal.target_pose.header.stamp = ros::Time::now();
72  goal.target_pose.header.frame_id = "map";
73  goal.target_pose.pose.position.x = 1.24;
74  goal.target_pose.pose.position.y = 0.65;
75  goal.target_pose.pose.position.z = 0.0;
76  goal.target_pose.pose.orientation.x = 0.0;
77  goal.target_pose.pose.orientation.y = 0.0;
78  goal.target_pose.pose.orientation.z = 0.0;
79  goal.target_pose.pose.orientation.w = 1.0;
80  return goal;
81  }
82 
86  planner_cspace_msgs::PlannerStatus status_;
87 };
88 
90 {
91  move_base_->sendGoal(CreateGoalInFree());
92  while (move_base_->getState().state_ !=
94  {
95  ros::Duration(1.0).sleep();
96  }
97  while (move_base_->getState().state_ ==
99  {
100  move_base_->cancelAllGoals();
101  ros::Duration(1.0).sleep();
102  }
103 
105  move_base_->getState().state_);
106  ASSERT_EQ(planner_cspace_msgs::PlannerStatus::GOING_WELL,
107  status_.error);
108  ASSERT_EQ(planner_cspace_msgs::PlannerStatus::DONE,
109  status_.status);
110 }
111 
112 int main(int argc, char** argv)
113 {
114  testing::InitGoogleTest(&argc, argv);
115  ros::init(argc, argv, "test_preempt");
116 
118  spinner.start();
119  int ret = RUN_ALL_TESTS();
120  spinner.stop();
121  ros::shutdown();
122  return ret;
123 }
std::shared_ptr< ActionClient > ActionClientPtr
TEST_F(PreemptTest, Preempt)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void spinner()
planner_cspace_msgs::PlannerStatus status_
move_base_msgs::MoveBaseGoal CreateGoalInFree()
ros::NodeHandle node_
ros::Subscriber status_sub_
void StatusCallback(const planner_cspace_msgs::PlannerStatus &msg)
static Time now()
ROSCPP_DECL void shutdown()
int main(int argc, char **argv)
ActionClientPtr move_base_
#define ROS_ERROR(...)


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