38 #ifndef COSTMAP_2D_FOOTPRINT_H 39 #define COSTMAP_2D_FOOTPRINT_H 42 #include <geometry_msgs/Polygon.h> 43 #include <geometry_msgs/PolygonStamped.h> 44 #include <geometry_msgs/Point.h> 45 #include <geometry_msgs/Point32.h> 58 double& min_dist,
double& max_dist);
63 geometry_msgs::Point
toPoint(geometry_msgs::Point32 pt);
68 geometry_msgs::Point32
toPoint32(geometry_msgs::Point pt);
73 geometry_msgs::Polygon
toPolygon(std::vector<geometry_msgs::Point> pts);
78 std::vector<geometry_msgs::Point>
toPointVector(geometry_msgs::Polygon polygon);
88 void transformFootprint(
double x,
double y,
double theta,
const std::vector<geometry_msgs::Point>& footprint_spec,
89 std::vector<geometry_msgs::Point>& oriented_footprint);
99 void transformFootprint(
double x,
double y,
double theta,
const std::vector<geometry_msgs::Point>& footprint_spec,
100 geometry_msgs::PolygonStamped & oriented_footprint);
105 void padFootprint(std::vector<geometry_msgs::Point>& footprint,
double padding);
118 bool makeFootprintFromString(
const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint);
138 const std::string& full_param_name);
147 #endif // COSTMAP_2D_FOOTPRINT_H std::vector< geometry_msgs::Point > makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue &footprint_xmlrpc, const std::string &full_param_name)
Create the footprint from the given XmlRpcValue.
std::vector< geometry_msgs::Point > makeFootprintFromParams(ros::NodeHandle &nh)
Read the ros-params "footprint" and/or "robot_radius" from the given NodeHandle using searchParam() t...
geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)
Convert Point to Point32.
void writeFootprintToParam(ros::NodeHandle &nh, const std::vector< geometry_msgs::Point > &footprint)
Write the current unpadded_footprint_ to the "footprint" parameter of the given NodeHandle so that dy...
bool makeFootprintFromString(const std::string &footprint_string, std::vector< geometry_msgs::Point > &footprint)
Make the footprint from the given string.
std::vector< geometry_msgs::Point > toPointVector(geometry_msgs::Polygon polygon)
Convert Polygon msg to vector of Points.
std::vector< geometry_msgs::Point > makeFootprintFromRadius(double radius)
Create a circular footprint from a given radius.
void padFootprint(std::vector< geometry_msgs::Point > &footprint, double padding)
Adds the specified amount of padding to the footprint (in place)
geometry_msgs::Polygon toPolygon(std::vector< geometry_msgs::Point > pts)
Convert vector of Points to Polygon msg.
geometry_msgs::Point toPoint(geometry_msgs::Point32 pt)
Convert Point32 to Point.
void transformFootprint(double x, double y, double theta, const std::vector< geometry_msgs::Point > &footprint_spec, std::vector< geometry_msgs::Point > &oriented_footprint)
Given a pose and base footprint, build the oriented footprint of the robot (list of Points) ...
void calculateMinAndMaxDistances(const std::vector< geometry_msgs::Point > &footprint, double &min_dist, double &max_dist)
Calculate the extreme distances for the footprint.