footprint.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Willow Garage, Inc.
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 Willow Garage, Inc. 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 
31 #include <boost/tokenizer.hpp>
32 #include <boost/foreach.hpp>
33 #include <boost/algorithm/string.hpp>
34 #include <costmap_2d/footprint.h>
36 #include<geometry_msgs/Point32.h>
37 
38 namespace costmap_2d
39 {
40 
41 void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
42 {
43  min_dist = std::numeric_limits<double>::max();
44  max_dist = 0.0;
45 
46  if (footprint.size() <= 2)
47  {
48  return;
49  }
50 
51  for (unsigned int i = 0; i < footprint.size() - 1; ++i)
52  {
53  // check the distance from the robot center point to the first vertex
54  double vertex_dist = distance(0.0, 0.0, footprint[i].x, footprint[i].y);
55  double edge_dist = distanceToLine(0.0, 0.0, footprint[i].x, footprint[i].y,
56  footprint[i + 1].x, footprint[i + 1].y);
57  min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
58  max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
59  }
60 
61  // we also need to do the last vertex and the first vertex
62  double vertex_dist = distance(0.0, 0.0, footprint.back().x, footprint.back().y);
63  double edge_dist = distanceToLine(0.0, 0.0, footprint.back().x, footprint.back().y,
64  footprint.front().x, footprint.front().y);
65  min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
66  max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
67 }
68 
69 geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)
70 {
71  geometry_msgs::Point32 point32;
72  point32.x = pt.x;
73  point32.y = pt.y;
74  point32.z = pt.z;
75  return point32;
76 }
77 
78 geometry_msgs::Point toPoint(geometry_msgs::Point32 pt)
79 {
80  geometry_msgs::Point point;
81  point.x = pt.x;
82  point.y = pt.y;
83  point.z = pt.z;
84  return point;
85 }
86 
87 geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts)
88 {
89  geometry_msgs::Polygon polygon;
90  for (int i = 0; i < pts.size(); i++){
91  polygon.points.push_back(toPoint32(pts[i]));
92  }
93  return polygon;
94 }
95 
96 std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
97 {
98  std::vector<geometry_msgs::Point> pts;
99  for (int i = 0; i < polygon.points.size(); i++)
100  {
101  pts.push_back(toPoint(polygon.points[i]));
102  }
103  return pts;
104 }
105 
106 void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
107  std::vector<geometry_msgs::Point>& oriented_footprint)
108 {
109  // build the oriented footprint at a given location
110  oriented_footprint.clear();
111  double cos_th = cos(theta);
112  double sin_th = sin(theta);
113  for (unsigned int i = 0; i < footprint_spec.size(); ++i)
114  {
115  geometry_msgs::Point new_pt;
116  new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
117  new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
118  oriented_footprint.push_back(new_pt);
119  }
120 }
121 
122 void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
123  geometry_msgs::PolygonStamped& oriented_footprint)
124 {
125  // build the oriented footprint at a given location
126  oriented_footprint.polygon.points.clear();
127  double cos_th = cos(theta);
128  double sin_th = sin(theta);
129  for (unsigned int i = 0; i < footprint_spec.size(); ++i)
130  {
131  geometry_msgs::Point32 new_pt;
132  new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
133  new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
134  oriented_footprint.polygon.points.push_back(new_pt);
135  }
136 }
137 
138 void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
139 {
140  // pad footprint in place
141  for (unsigned int i = 0; i < footprint.size(); i++)
142  {
143  geometry_msgs::Point& pt = footprint[ i ];
144  pt.x += sign0(pt.x) * padding;
145  pt.y += sign0(pt.y) * padding;
146  }
147 }
148 
149 
150 std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
151 {
152  std::vector<geometry_msgs::Point> points;
153 
154  // Loop over 16 angles around a circle making a point each time
155  int N = 16;
156  geometry_msgs::Point pt;
157  for (int i = 0; i < N; ++i)
158  {
159  double angle = i * 2 * M_PI / N;
160  pt.x = cos(angle) * radius;
161  pt.y = sin(angle) * radius;
162 
163  points.push_back(pt);
164  }
165 
166  return points;
167 }
168 
169 
170 bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint)
171 {
172  std::string error;
173  std::vector<std::vector<float> > vvf = parseVVF(footprint_string, error);
174 
175  if (error != "")
176  {
177  ROS_ERROR("Error parsing footprint parameter: '%s'", error.c_str());
178  ROS_ERROR(" Footprint string was '%s'.", footprint_string.c_str());
179  return false;
180  }
181 
182  // convert vvf into points.
183  if (vvf.size() < 3)
184  {
185  ROS_ERROR("You must specify at least three points for the robot footprint, reverting to previous footprint.");
186  return false;
187  }
188  footprint.reserve(vvf.size());
189  for (unsigned int i = 0; i < vvf.size(); i++)
190  {
191  if (vvf[ i ].size() == 2)
192  {
193  geometry_msgs::Point point;
194  point.x = vvf[ i ][ 0 ];
195  point.y = vvf[ i ][ 1 ];
196  point.z = 0;
197  footprint.push_back(point);
198  }
199  else
200  {
201  ROS_ERROR("Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.",
202  int(vvf[ i ].size()));
203  return false;
204  }
205  }
206 
207  return true;
208 }
209 
210 
211 
212 std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
213 {
214  std::string full_param_name;
215  std::string full_radius_param_name;
216  std::vector<geometry_msgs::Point> points;
217 
218  if (nh.searchParam("footprint", full_param_name))
219  {
220  XmlRpc::XmlRpcValue footprint_xmlrpc;
221  nh.getParam(full_param_name, footprint_xmlrpc);
222  if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
223  footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
224  {
225  if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
226  {
227  writeFootprintToParam(nh, points);
228  return points;
229  }
230  }
231  else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
232  {
233  points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
234  writeFootprintToParam(nh, points);
235  return points;
236  }
237  }
238 
239  if (nh.searchParam("robot_radius", full_radius_param_name))
240  {
241  double robot_radius;
242  nh.param(full_radius_param_name, robot_radius, 1.234);
243  points = makeFootprintFromRadius(robot_radius);
244  nh.setParam("robot_radius", robot_radius);
245  }
246  // Else neither param was found anywhere this knows about, so
247  // defaults will come from dynamic_reconfigure stuff, set in
248  // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
249  return points;
250 }
251 
252 void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint)
253 {
254  std::ostringstream oss;
255  bool first = true;
256  for (unsigned int i = 0; i < footprint.size(); i++)
257  {
258  geometry_msgs::Point p = footprint[ i ];
259  if (first)
260  {
261  oss << "[[" << p.x << "," << p.y << "]";
262  first = false;
263  }
264  else
265  {
266  oss << ",[" << p.x << "," << p.y << "]";
267  }
268  }
269  oss << "]";
270  nh.setParam("footprint", oss.str().c_str());
271 }
272 
273 double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
274 {
275  // Make sure that the value we're looking at is either a double or an int.
276  if (value.getType() != XmlRpc::XmlRpcValue::TypeInt &&
278  {
279  std::string& value_string = value;
280  ROS_FATAL("Values in the footprint specification (param %s) must be numbers. Found value %s.",
281  full_param_name.c_str(), value_string.c_str());
282  throw std::runtime_error("Values in the footprint specification must be numbers");
283  }
284  return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
285 }
286 
287 std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
288  const std::string& full_param_name)
289 {
290  // Make sure we have an array of at least 3 elements.
291  if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
292  footprint_xmlrpc.size() < 3)
293  {
294  ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s",
295  full_param_name.c_str(), std::string(footprint_xmlrpc).c_str());
296  throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
297  "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
298  }
299 
300  std::vector<geometry_msgs::Point> footprint;
301  geometry_msgs::Point pt;
302 
303  for (int i = 0; i < footprint_xmlrpc.size(); ++i)
304  {
305  // Make sure each element of the list is an array of size 2. (x and y coordinates)
306  XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
307  if (point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
308  point.size() != 2)
309  {
310  ROS_FATAL("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
311  "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.",
312  full_param_name.c_str());
313  throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: "
314  "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
315  }
316 
317  pt.x = getNumberFromXMLRPC(point[ 0 ], full_param_name);
318  pt.y = getNumberFromXMLRPC(point[ 1 ], full_param_name);
319 
320  footprint.push_back(pt);
321  }
322  return footprint;
323 }
324 
325 } // end namespace costmap_2d
std::vector< geometry_msgs::Point > makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue &footprint_xmlrpc, const std::string &full_param_name)
Create the footprint from the given XmlRpcValue.
Definition: footprint.cpp:287
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...
Definition: footprint.cpp:212
#define ROS_FATAL(...)
std::vector< std::vector< float > > parseVVF(const std::string &input, std::string &error_return)
Parse a vector of vectors of floats from a string.
geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)
Convert Point to Point32.
Definition: footprint.cpp:69
int size() const
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...
Definition: footprint.cpp:252
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
Type const & getType() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
bool makeFootprintFromString(const std::string &footprint_string, std::vector< geometry_msgs::Point > &footprint)
Make the footprint from the given string.
Definition: footprint.cpp:170
double sign0(double x)
Same as sign(x) but returns 0 if x is 0.
Definition: costmap_math.h:53
std::vector< geometry_msgs::Point > toPointVector(geometry_msgs::Polygon polygon)
Convert Polygon msg to vector of Points.
Definition: footprint.cpp:96
double distance(double x0, double y0, double x1, double y1)
Definition: costmap_math.h:58
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
std::vector< geometry_msgs::Point > makeFootprintFromRadius(double radius)
Create a circular footprint from a given radius.
Definition: footprint.cpp:150
void padFootprint(std::vector< geometry_msgs::Point > &footprint, double padding)
Adds the specified amount of padding to the footprint (in place)
Definition: footprint.cpp:138
TFSIMD_FORCE_INLINE const tfScalar & x() const
geometry_msgs::Polygon toPolygon(std::vector< geometry_msgs::Point > pts)
Convert vector of Points to Polygon msg.
Definition: footprint.cpp:87
geometry_msgs::Point toPoint(geometry_msgs::Point32 pt)
Convert Point32 to Point.
Definition: footprint.cpp:78
bool getParam(const std::string &key, std::string &s) const
double getNumberFromXMLRPC(XmlRpc::XmlRpcValue &value, const std::string &full_param_name)
Definition: footprint.cpp:273
#define ROS_ERROR(...)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
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) ...
Definition: footprint.cpp:106
void calculateMinAndMaxDistances(const std::vector< geometry_msgs::Point > &footprint, double &min_dist, double &max_dist)
Calculate the extreme distances for the footprint.
Definition: footprint.cpp:41


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:17