grid_metric_converter.h
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 #ifndef PLANNER_CSPACE_PLANNER_3D_GRID_METRIC_CONVERTER_H
31 #define PLANNER_CSPACE_PLANNER_3D_GRID_METRIC_CONVERTER_H
32 
33 #include <cmath>
34 #include <list>
35 #include <memory>
36 
37 #include <costmap_cspace_msgs/MapMetaData3D.h>
38 #include <nav_msgs/Path.h>
40 
42 
43 namespace planner_cspace
44 {
45 namespace planner_3d
46 {
47 namespace grid_metric_converter
48 {
49 template <typename T>
51  const costmap_cspace_msgs::MapMetaData3D& map_info,
52  const T x, const T y, const T yaw,
53  float& gx, float& gy, float& gyaw)
54 {
55  static_assert(
56  std::is_same<float, T>() || std::is_same<int, T>(), "T must be float or int");
57 
58  gx = (x + 0.5) * map_info.linear_resolution + map_info.origin.position.x;
59  gy = (y + 0.5) * map_info.linear_resolution + map_info.origin.position.y;
60  gyaw = yaw * map_info.angular_resolution;
61 }
62 inline void metric2Grid(
63  const costmap_cspace_msgs::MapMetaData3D& map_info,
64  int& x, int& y, int& yaw,
65  const float gx, const float gy, const float gyaw)
66 {
67  x = static_cast<int>(std::floor((gx - map_info.origin.position.x) / map_info.linear_resolution));
68  y = static_cast<int>(std::floor((gy - map_info.origin.position.y) / map_info.linear_resolution));
69  yaw = std::lround(gyaw / map_info.angular_resolution);
70 }
71 inline void metric2Grid(
72  const costmap_cspace_msgs::MapMetaData3D& map_info,
73  float& x, float& y, float& yaw,
74  const float gx, const float gy, const float gyaw)
75 {
76  x = (gx - map_info.origin.position.x) / map_info.linear_resolution;
77  y = (gy - map_info.origin.position.y) / map_info.linear_resolution;
78  yaw = gyaw / map_info.angular_resolution;
79 }
80 
81 template <template <class, class> class STL_CONTAINER = std::list>
83  const costmap_cspace_msgs::MapMetaData3D& map_info,
84  const STL_CONTAINER<CyclicVecFloat<3, 2>,
86  nav_msgs::Path& path)
87 {
88  for (const auto& p : path_grid)
89  {
90  float x, y, yaw;
91  grid2Metric(map_info, p[0], p[1], p[2], x, y, yaw);
92  geometry_msgs::PoseStamped ps;
93  ps.header = path.header;
94 
95  ps.pose.position.x = x;
96  ps.pose.position.y = y;
97  ps.pose.position.z = 0;
98  ps.pose.orientation =
99  tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
100  path.poses.push_back(ps);
101  }
102 }
103 } // namespace grid_metric_converter
104 } // namespace planner_3d
105 } // namespace planner_cspace
106 
107 #endif // PLANNER_CSPACE_PLANNER_3D_GRID_METRIC_CONVERTER_H
TFSIMD_FORCE_INLINE const tfScalar & y() const
void metric2Grid(const costmap_cspace_msgs::MapMetaData3D &map_info, int &x, int &y, int &yaw, const float gx, const float gy, const float gyaw)
TFSIMD_FORCE_INLINE const tfScalar & x() const
void grid2MetricPath(const costmap_cspace_msgs::MapMetaData3D &map_info, const STL_CONTAINER< CyclicVecFloat< 3, 2 >, std::allocator< CyclicVecFloat< 3, 2 >>> &path_grid, nav_msgs::Path &path)
B toMsg(const A &a)
void grid2Metric(const costmap_cspace_msgs::MapMetaData3D &map_info, const T x, const T y, const T yaw, float &gx, float &gy, float &gyaw)


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