41 geometry_msgs::Twist
twist2Dto3D(
const nav_2d_msgs::Twist2D& cmd_vel_2d)
43 geometry_msgs::Twist cmd_vel;
44 cmd_vel.linear.x = cmd_vel_2d.x;
45 cmd_vel.linear.y = cmd_vel_2d.y;
46 cmd_vel.angular.z = cmd_vel_2d.theta;
51 nav_2d_msgs::Twist2D
twist3Dto2D(
const geometry_msgs::Twist& cmd_vel)
53 nav_2d_msgs::Twist2D cmd_vel_2d;
54 cmd_vel_2d.x = cmd_vel.linear.x;
55 cmd_vel_2d.y = cmd_vel.linear.y;
56 cmd_vel_2d.theta = cmd_vel.angular.z;
62 nav_2d_msgs::Point2D output;
70 nav_2d_msgs::Point2D output;
78 geometry_msgs::Point output;
86 geometry_msgs::Point32 output;
94 nav_2d_msgs::Pose2DStamped pose2d;
95 pose2d.header.stamp = pose.
stamp_;
97 pose2d.pose.x = pose.getOrigin().getX();
98 pose2d.pose.y = pose.getOrigin().getY();
99 pose2d.pose.theta =
tf::getYaw(pose.getRotation());
105 nav_2d_msgs::Pose2DStamped pose2d;
106 pose2d.header = pose.header;
107 pose2d.pose.x = pose.pose.position.x;
108 pose2d.pose.y = pose.pose.position.y;
109 pose2d.pose.theta =
tf::getYaw(pose.pose.orientation);
115 geometry_msgs::Pose pose;
116 pose.position.x = pose2d.x;
117 pose.position.y = pose2d.y;
124 geometry_msgs::PoseStamped pose;
125 pose.header = pose2d.header;
131 const std::string& frame,
const ros::Time& stamp)
133 geometry_msgs::PoseStamped pose;
134 pose.header.frame_id = frame;
135 pose.header.stamp = stamp;
136 pose.pose.position.x = pose2d.x;
137 pose.pose.position.y = pose2d.y;
144 nav_2d_msgs::Path2D path2d;
145 path2d.header = path.header;
146 nav_2d_msgs::Pose2DStamped stamped_2d;
147 path2d.poses.resize(path.poses.size());
148 for (
unsigned int i = 0; i < path.poses.size(); i++)
151 path2d.poses[i] = stamped_2d.pose;
156 nav_msgs::Path
posesToPath(
const std::vector<geometry_msgs::PoseStamped>& poses)
162 path.poses.resize(poses.size());
163 path.header.frame_id = poses[0].header.frame_id;
164 path.header.stamp = poses[0].header.stamp;
165 for (
unsigned int i = 0; i < poses.size(); i++)
167 path.poses[i] = poses[i];
172 nav_2d_msgs::Path2D
posesToPath2D(
const std::vector<geometry_msgs::PoseStamped>& poses)
174 nav_2d_msgs::Path2D path;
178 nav_2d_msgs::Pose2DStamped pose;
179 path.poses.resize(poses.size());
180 path.header.frame_id = poses[0].header.frame_id;
181 path.header.stamp = poses[0].header.stamp;
182 for (
unsigned int i = 0; i < poses.size(); i++)
185 path.poses[i] = pose.pose;
191 nav_msgs::Path
poses2DToPath(
const std::vector<geometry_msgs::Pose2D>& poses,
192 const std::string& frame,
const ros::Time& stamp)
195 path.poses.resize(poses.size());
196 path.header.frame_id = frame;
197 path.header.stamp = stamp;
198 for (
unsigned int i = 0; i < poses.size(); i++)
208 path.header = path2d.header;
209 path.poses.resize(path2d.poses.size());
210 for (
unsigned int i = 0; i < path.poses.size(); i++)
212 path.poses[i].header = path2d.header;
218 geometry_msgs::Polygon
polygon2Dto3D(
const nav_2d_msgs::Polygon2D& polygon_2d)
220 geometry_msgs::Polygon polygon;
221 polygon.points.reserve(polygon_2d.points.size());
222 for (
const auto& pt : polygon_2d.points)
229 nav_2d_msgs::Polygon2D
polygon3Dto2D(
const geometry_msgs::Polygon& polygon_3d)
231 nav_2d_msgs::Polygon2D polygon;
232 polygon.points.reserve(polygon_3d.points.size());
233 for (
const auto& pt : polygon_3d.points)
240 geometry_msgs::PolygonStamped
polygon2Dto3D(
const nav_2d_msgs::Polygon2DStamped& polygon_2d)
242 geometry_msgs::PolygonStamped polygon;
243 polygon.header = polygon_2d.header;
248 nav_2d_msgs::Polygon2DStamped
polygon3Dto2D(
const geometry_msgs::PolygonStamped& polygon_3d)
250 nav_2d_msgs::Polygon2DStamped polygon;
251 polygon.header = polygon_3d.header;
258 nav_2d_msgs::NavGridInfo msg;
259 msg.width = info.
width;
271 info.
width = msg.width;
285 info.
width = metadata.width;
286 info.
height = metadata.height;
287 info.
origin_x = metadata.origin.position.x;
288 info.
origin_y = metadata.origin.position.y;
289 if (std::abs(
tf::getYaw(metadata.origin.orientation)) > 1e-5)
292 "Conversion from MapMetaData to NavGridInfo encountered a non-zero rotation. Ignoring.");
299 geometry_msgs::Pose origin;
302 origin.orientation.w = 1.0;
308 geometry_msgs::Pose2D origin;
316 nav_msgs::MapMetaData metadata;
318 metadata.width = info.
width;
319 metadata.height = info.
height;
326 nav_2d_msgs::UIntBounds msg;
nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData &metadata, const std::string &frame)
geometry_msgs::Point32 pointToPoint32(const nav_2d_msgs::Point2D &point)
unsigned int getMaxX() const
geometry_msgs::Pose pose2DToPose(const geometry_msgs::Pose2D &pose2d)
A set of utility functions for Bounds objects interacting with other messages/types.
#define ROS_WARN_NAMED(name,...)
unsigned int getMinX() const
unsigned int getMinY() const
static double getYaw(const Quaternion &bt_q)
nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo &info)
nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path)
geometry_msgs::Point pointToPoint3D(const nav_2d_msgs::Point2D &point)
nav_grid::NavGridInfo fromMsg(const nav_2d_msgs::NavGridInfo &msg)
geometry_msgs::Twist twist2Dto3D(const nav_2d_msgs::Twist2D &cmd_vel_2d)
nav_msgs::Path posesToPath(const std::vector< geometry_msgs::PoseStamped > &poses)
nav_2d_msgs::Path2D posesToPath2D(const std::vector< geometry_msgs::PoseStamped > &poses)
nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point &point)
nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped &pose)
nav_msgs::Path poses2DToPath(const std::vector< geometry_msgs::Pose2D > &poses, const std::string &frame, const ros::Time &stamp)
geometry_msgs::PoseStamped pose2DToPoseStamped(const nav_2d_msgs::Pose2DStamped &pose2d)
geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info)
nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped< tf::Pose > &pose)
geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo &info)
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
geometry_msgs::Polygon polygon2Dto3D(const nav_2d_msgs::Polygon2D &polygon_2d)
nav_2d_msgs::Polygon2D polygon3Dto2D(const geometry_msgs::Polygon &polygon_3d)
unsigned int getMaxY() const
nav_2d_msgs::Twist2D twist3Dto2D(const geometry_msgs::Twist &cmd_vel)