【ROS】costmap之（3）footprint

最新推荐文章于 2026-08-16 08:23:33 发布

原创最新推荐文章于 2026-08-16 08:23:33 发布 · 1.7k 阅读

5 ·

本内容遵循CC 4.0 BY-SA版权协议

收录于

ROS

本文介绍了ROS costmap_2d模块中关于footprint的相关函数，包括计算轮廓最小最大距离、点转换、轮廓变换、内边距添加、根据半径、字符串、参数生成轮廓等，以及从XMLRPC值获取数值的方法。

costmap_2d: footprint.cpp

costmap_2d: footprint.h

costmap_2d: footprint_tests.cpp

1、函数[void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint,

double& min_dist, double& max_dist)]

功能：计算机器人轮廓的最小距离和最大距离

通过计算原点与顶点距离、原点与线段(顶点-顶点)距离，算出轮廓的最小距离和最大距离。

2、函数[geometry_msgs::Point toPoint(geometry_msgs::Point32 pt)]

功能：Point32转为Point(实际是float32转为float64)

3、函数[geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)]

功能：Point转为Point32(实际是float64转为float32)

4、函数[geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts)]

功能：vector容器里的Point转为geometry_msgs::Polygon里的points

一般用于储存机器人轮廓的顶点

5、函数[std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)]

功能：geometry_msgs::Polygon里的points转为vector容器里的Point

一般用于储存机器人轮廓的顶点

6、函数[void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec, std::vector<geometry_msgs::Point>& oriented_footprint)]

功能：把footprint_spec轮廓变换到坐标系(x,y,theta)，输出对应的轮廓oriented_footprint

7、函数[void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec, geometry_msgs::PolygonStamped& oriented_footprint)]

功能：把footprint_spec轮廓变换到坐标系(x,y,theta)，输出对应的轮廓oriented_footprint

8、函数[void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)]

功能：轮廓footprint添加内边距，边距为padding。

9、函数[std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)]

功能：根据半径生成轮廓

10、函数[bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint)]

功能：根据字符串生成轮廓

11、函数[std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)]

功能：根据ROS参数系统footprint参数生成轮廓

12、函数[void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint)]

功能：根据输入的轮廓footprint设置ROS参数

13、函数[double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)]