Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef PLANNER_CSPACE_PLANNER_3D_GRID_METRIC_CONVERTER_H
00031 #define PLANNER_CSPACE_PLANNER_3D_GRID_METRIC_CONVERTER_H
00032
00033 #include <costmap_cspace_msgs/MapMetaData3D.h>
00034 #include <nav_msgs/Path.h>
00035 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00036
00037 #include <planner_cspace/cyclic_vec.h>
00038
00039 #include <list>
00040 #include <memory>
00041
00042 namespace grid_metric_converter
00043 {
00044 template <typename T>
00045 void grid2Metric(
00046 const costmap_cspace_msgs::MapMetaData3D& map_info,
00047 const T x, const T y, const T yaw,
00048 float& gx, float& gy, float& gyaw)
00049 {
00050 static_assert(
00051 std::is_same<float, T>() || std::is_same<int, T>(), "T must be float or int");
00052
00053 gx = (x + 0.5) * map_info.linear_resolution + map_info.origin.position.x;
00054 gy = (y + 0.5) * map_info.linear_resolution + map_info.origin.position.y;
00055 gyaw = yaw * map_info.angular_resolution;
00056 }
00057 inline void metric2Grid(
00058 const costmap_cspace_msgs::MapMetaData3D& map_info,
00059 int& x, int& y, int& yaw,
00060 const float gx, const float gy, const float gyaw)
00061 {
00062 x = static_cast<int>(floor((gx - map_info.origin.position.x) / map_info.linear_resolution));
00063 y = static_cast<int>(floor((gy - map_info.origin.position.y) / map_info.linear_resolution));
00064 yaw = lroundf(gyaw / map_info.angular_resolution);
00065 }
00066
00067 template <template <class, class> class STL_CONTAINER = std::list>
00068 void grid2MetricPath(
00069 const costmap_cspace_msgs::MapMetaData3D& map_info,
00070 const STL_CONTAINER<CyclicVecFloat<3, 2>,
00071 std::allocator<CyclicVecFloat<3, 2>>>& path_grid,
00072 nav_msgs::Path& path)
00073 {
00074 for (const auto& p : path_grid)
00075 {
00076 float x, y, yaw;
00077 grid2Metric(map_info, p[0], p[1], p[2], x, y, yaw);
00078 geometry_msgs::PoseStamped ps;
00079 ps.header = path.header;
00080
00081 ps.pose.position.x = x;
00082 ps.pose.position.y = y;
00083 ps.pose.position.z = 0;
00084 ps.pose.orientation =
00085 tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
00086 path.poses.push_back(ps);
00087 }
00088 }
00089 }
00090
00091 #endif // PLANNER_CSPACE_PLANNER_3D_GRID_METRIC_CONVERTER_H