test_debug_outputs.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 <ros/ros.h>
31 
32 #include <nav_msgs/OccupancyGrid.h>
33 #include <nav_msgs/Path.h>
34 #include <planner_cspace_msgs/PlannerStatus.h>
35 #include <sensor_msgs/PointCloud.h>
36 
37 #include <gtest/gtest.h>
38 
39 class DebugOutputsTest : public ::testing::Test
40 {
41 public:
44  , cnt_path_(0)
45  , cnt_hysteresis_(0)
46  , cnt_remembered_(0)
47  {
48  sub_status_ = nh_.subscribe("/planner_3d/status", 1, &DebugOutputsTest::cbStatus, this);
49  sub_path_ = nh_.subscribe("path", 1, &DebugOutputsTest::cbPath, this);
50  sub_hysteresis_ = nh_.subscribe("/planner_3d/hysteresis_map", 1, &DebugOutputsTest::cbHysteresis, this);
51  sub_remembered_ = nh_.subscribe("/planner_3d/remembered_map", 1, &DebugOutputsTest::cbRemembered, this);
52  sub_distance_ = nh_.subscribe("/planner_3d/distance_map", 1, &DebugOutputsTest::cbDistance, this);
53 
54  // Wait planner
55  while (ros::ok())
56  {
57  ros::Duration(0.1).sleep();
58  ros::spinOnce();
59  if (cnt_planner_ready_ > 5 && cnt_path_ > 5)
60  break;
61  }
62  map_hysteresis_ = nullptr;
63  map_remembered_ = nullptr;
64  map_distance_ = nullptr;
65  cnt_hysteresis_ = 0;
66  cnt_remembered_ = 0;
67  cnt_distance_ = 0;
68 
69  // Wait receiving the messages
70  while (ros::ok())
71  {
72  ros::Duration(0.1).sleep();
73  ros::spinOnce();
74  // First hysteresis map doesn't have previous path information.
75  if (cnt_hysteresis_ > 2 && cnt_remembered_ > 2)
76  break;
77  }
78  }
79  void showMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
80  {
81  std::cerr << std::hex;
82  for (size_t y = 0; y < msg->info.height; ++y)
83  {
84  for (size_t x = 0; x < msg->info.width; ++x)
85  {
86  std::cerr << (msg->data[x + y * msg->info.width] / 11);
87  }
88  std::cerr << std::endl;
89  }
90  std::cerr << std::dec;
91 
92  std::streamsize ss = std::cerr.precision();
93  std::cerr << "path:" << std::endl
94  << std::setprecision(3);
95  for (const auto& p : path_->poses)
96  std::cerr << p.pose.position.x << ", " << p.pose.position.y << ", " << std::endl;
97 
98  std::cerr << std::setprecision(ss);
99  }
100 
101 protected:
102  void cbHysteresis(const nav_msgs::OccupancyGrid::ConstPtr& msg)
103  {
104  cnt_hysteresis_++;
105  map_hysteresis_ = msg;
106  }
107  void cbRemembered(const nav_msgs::OccupancyGrid::ConstPtr& msg)
108  {
109  cnt_remembered_++;
110  map_remembered_ = msg;
111  }
112  void cbDistance(const sensor_msgs::PointCloud::ConstPtr& msg)
113  {
114  cnt_distance_++;
115  map_distance_ = msg;
116  }
117  void cbStatus(const planner_cspace_msgs::PlannerStatus::ConstPtr& msg)
118  {
119  if (msg->error == planner_cspace_msgs::PlannerStatus::GOING_WELL &&
120  msg->status == planner_cspace_msgs::PlannerStatus::DOING)
122  }
123  void cbPath(const nav_msgs::Path::ConstPtr& msg)
124  {
125  if (msg->poses.size() > 0)
126  {
127  ++cnt_path_;
128  path_ = msg;
129  }
130  }
131 
133  nav_msgs::OccupancyGrid::ConstPtr map_hysteresis_;
134  nav_msgs::OccupancyGrid::ConstPtr map_remembered_;
135  sensor_msgs::PointCloud::ConstPtr map_distance_;
136  nav_msgs::Path::ConstPtr path_;
147 };
148 
150 {
151  int x;
152  int y;
153  char value;
154 };
155 
157 {
158  ASSERT_TRUE(static_cast<bool>(map_hysteresis_));
159 
160  // Robot is at (25, 4) and goal is at (10, 4)
161  const PositionAndValue data_set[] =
162  {
163  { 25, 4, 0 },
164  { 20, 4, 0 },
165  { 15, 4, 0 },
166  { 10, 4, 0 },
167  { 25, 1, 100 },
168  { 20, 1, 100 },
169  { 15, 1, 100 },
170  { 10, 1, 100 },
171  { 25, 7, 100 },
172  { 20, 7, 100 },
173  { 15, 7, 100 },
174  { 10, 7, 100 },
175  };
176  for (auto data : data_set)
177  {
178  const size_t addr = data.x + data.y * map_hysteresis_->info.width;
179  EXPECT_EQ(data.value, map_hysteresis_->data[addr]) << "x: " << data.x << ", y: " << data.y;
180  }
181  if (::testing::Test::HasFailure())
182  {
184  }
185 }
186 
188 {
189  ASSERT_TRUE(static_cast<bool>(map_remembered_));
190 
191  // Robot is at (25, 5) and obstacles are placed at
192  // (0, 0)-(31, 0) and (18, 10)-(31, 10).
193  // Costmap is expanded by 1 grid.
194  const PositionAndValue data_set[] =
195  {
196  { 17, 0, 100 }, // occupied
197  { 17, 1, 100 }, // expanded
198  { 17, 2, 0 }, // free
199  { 17, 8, 0 }, // free
200  { 17, 9, 100 }, // expanded
201  { 17, 10, 100 }, // occupied
202  };
203  for (auto data : data_set)
204  {
205  const size_t addr = data.x + data.y * map_remembered_->info.width;
206  EXPECT_EQ(data.value, map_remembered_->data[addr]) << "x: " << data.x << ", y: " << data.y;
207  }
208  if (::testing::Test::HasFailure())
209  {
211  }
212 }
213 
215 {
216  ASSERT_TRUE(static_cast<bool>(map_distance_));
217 
218  // Robot is at (2.5, 0.5) and goal is at (1.0, 0.5)
219  for (size_t i = 0; i < map_distance_->points.size(); ++i)
220  {
221  const float x = map_distance_->points[i].x;
222  const float y = map_distance_->points[i].y;
223  const float d = map_distance_->channels[0].values[i];
224  if (std::abs(y - 0.5) < 0.05)
225  {
226  const float dist_from_goal = std::abs(x - 1.0);
227  ASSERT_NEAR(dist_from_goal, d, 0.15);
228  }
229  }
230 }
231 
232 int main(int argc, char** argv)
233 {
234  testing::InitGoogleTest(&argc, argv);
235  ros::init(argc, argv, "test_debug_outputs");
236 
237  return RUN_ALL_TESTS();
238 }
d
nav_msgs::Path::ConstPtr path_
ros::Subscriber sub_remembered_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void cbPath(const nav_msgs::Path::ConstPtr &msg)
nav_msgs::OccupancyGrid::ConstPtr map_remembered_
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void cbHysteresis(const nav_msgs::OccupancyGrid::ConstPtr &msg)
void cbStatus(const planner_cspace_msgs::PlannerStatus::ConstPtr &msg)
data
void cbDistance(const sensor_msgs::PointCloud::ConstPtr &msg)
TFSIMD_FORCE_INLINE const tfScalar & y() const
TEST_F(DebugOutputsTest, Hysteresis)
ros::Subscriber sub_status_
ROSCPP_DECL bool ok()
TFSIMD_FORCE_INLINE const tfScalar & x() const
nav_msgs::OccupancyGrid::ConstPtr map_hysteresis_
sensor_msgs::PointCloud::ConstPtr map_distance_
void showMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
void cbRemembered(const nav_msgs::OccupancyGrid::ConstPtr &msg)
ros::Subscriber sub_distance_
int main(int argc, char **argv)
ROSCPP_DECL void spinOnce()
ros::Subscriber sub_path_
ros::Subscriber sub_hysteresis_


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