test_navigate_boundary.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019, 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 <cstddef>
31 #include <memory>
32 
33 #include <ros/ros.h>
36 #include <nav_msgs/Path.h>
38 #include <move_base_msgs/MoveBaseAction.h>
39 
40 #include <gtest/gtest.h>
41 
42 class NavigateBoundary : public ::testing::Test
43 {
44 protected:
46  using ActionClientPtr = std::shared_ptr<ActionClient>;
47 
51  nav_msgs::Path::ConstPtr path_;
53 
55  {
56  move_base_ = std::make_shared<ActionClient>("/move_base");
57  if (!move_base_->waitForServer(ros::Duration(10.0)))
58  {
59  ROS_ERROR("Failed to connect move_base action");
60  exit(EXIT_FAILURE);
61  }
62  }
63 
64  void publishTransform(const double x, const double y)
65  {
66  geometry_msgs::TransformStamped trans;
67  trans.header.stamp = ros::Time::now();
68  trans.header.frame_id = "odom";
69  trans.child_frame_id = "base_link";
70  trans.transform.translation.x = x;
71  trans.transform.translation.y = y;
72  trans.transform.rotation.w = 1.0;
73  tfb_.sendTransform(trans);
74  }
75  virtual void SetUp()
76  {
77  sub_path_ = nh_.subscribe("path", 1, &NavigateBoundary::cbPath, this);
78 
79  publishTransform(1.0, 0.6);
80  ros::Duration(0.5).sleep();
81 
82  move_base_msgs::MoveBaseGoal goal;
83  goal.target_pose.header.frame_id = "map";
84  goal.target_pose.header.stamp = ros::Time::now();
85  goal.target_pose.pose.orientation.w = 1;
86  goal.target_pose.pose.position.x = 1.4;
87  goal.target_pose.pose.position.y = 0.6;
88  move_base_->sendGoal(goal);
89  ros::Duration(0.5).sleep();
90  }
91  void cbPath(const nav_msgs::Path::ConstPtr& msg)
92  {
93  path_ = msg;
94  }
95 };
96 
97 TEST_F(NavigateBoundary, StartPositionScan)
98 {
99  // map width/height is 32px * 0.1m = 3.2m
100  for (double x = -10; x < 13; x += 2.0)
101  {
102  for (double y = -10; y < 13; y += 2.0)
103  {
104  publishTransform(x, y);
105 
106  path_ = nullptr;
107  for (int i = 0; i < 100; ++i)
108  {
109  ros::Duration(0.05).sleep();
110  ros::spinOnce();
111  if (path_)
112  break;
113  }
114  // Planner must publish at least empty path if alive.
115  ASSERT_TRUE(static_cast<bool>(path_));
116  }
117  }
118 }
119 
120 int main(int argc, char** argv)
121 {
122  testing::InitGoogleTest(&argc, argv);
123  ros::init(argc, argv, "test_navigate_boundary");
124 
125  return RUN_ALL_TESTS();
126 }
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
nav_msgs::Path::ConstPtr path_
bool sleep() const
void publishTransform(const double x, const double y)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
void sendTransform(const geometry_msgs::TransformStamped &transform)
int main(int argc, char **argv)
ActionClientPtr move_base_
static Time now()
void cbPath(const nav_msgs::Path::ConstPtr &msg)
TEST_F(NavigateBoundary, StartPositionScan)
tf2_ros::TransformBroadcaster tfb_
ROSCPP_DECL void spinOnce()
std::shared_ptr< ActionClient > ActionClientPtr
#define ROS_ERROR(...)


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