route_speeds.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2017, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
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 Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from 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 <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
30 
31 #include <unordered_map>
32 
35 #include <swri_route_util/util.h>
36 
37 namespace smu = swri_math_util;
38 namespace stu = swri_transform_util;
39 namespace mcm = marti_common_msgs;
40 namespace mnm = marti_nav_msgs;
41 
42 namespace swri_route_util
43 {
44 // Convenience function to add a key/value pair to a KeyValueArray message.
45 static void addItem(mcm::KeyValueArray &m, const std::string &key, const std::string &value)
46 {
47  m.items.emplace_back();
48  m.items.back().key = key;
49  m.items.back().value = value;
50 }
51 
53  :
54  use_speed_from_accel_constant_(true),
55  max_lateral_accel_mss_(0.2),
56  curvature_filter_size_(20.0)
57 {
58 }
59 
61 {
62  // 20.0 seems to be a good value from looking through 17 recorded routes we
63  // have with and without Omnistar corrections.
64  swri::param(pnh, "curvature_filter_size", curvature_filter_size_, 20.0);
65 
66 
67  swri::param(pnh, "lateral_acceleration_mode",
69  swri::param(pnh, "max_lateral_acceleration",
71 
72  if (!speed_curve_.readFromParameter(pnh, "curvature_vs_speed", true)) {
73  ROS_ERROR("Failed to load speed/curve parameter. Forcing lateral acceleration mode.");
75  } else {
76  ROS_INFO("Loaded speed vs curvature curve (%s)", speed_curve_.interpolationTypeString().c_str());
77  for (size_t i = 0; i < speed_curve_.numPoints(); i++) {
78  std::pair<double, double> pt = speed_curve_.getPoint(i);
79  ROS_INFO(" %zu -- %f [1/m] vs %f [m/s]", i, pt.first, pt.second);
80  }
81  }
82 }
83 
84 void SpeedForCurvatureParameters::loadFromConfig(const mcm::KeyValueArray &config)
85 {
86  std::unordered_map<std::string, std::string> config_map;
87  for (size_t i = 0; i < config.items.size(); ++i) {
88  config_map[config.items[i].key] = config.items[i].value;
89  }
90 
91  if (config_map.count("curvature_filter_size")) {
92  curvature_filter_size_ = boost::lexical_cast<double>(config_map.at("curvature_filter_size"));
93  ROS_INFO("Setting curvature_filter_size to %lf", curvature_filter_size_);
94  config_map.erase("curvature_filter_size");
95  }
96 
97  if (config_map.count("lateral_acceleration_mode")) {
99  boost::lexical_cast<int>(config_map.at("lateral_acceleration_mode"));
100  ROS_INFO("Setting lateral acceleration mode to %s",
101  use_speed_from_accel_constant_ ? "true" : "false");
102  config_map.erase("lateral_acceleration_mode");
103  }
104 
105  if (config_map.count("max_lateral_acceleration")) {
106  max_lateral_accel_mss_ = boost::lexical_cast<double>(config_map.at("max_lateral_acceleration"));
107  ROS_INFO("Setting max_lateral_acceleration to %lf", max_lateral_accel_mss_);
108  config_map.erase("max_lateral_acceleration");
109  }
110 
111  if (config_map.count("curvature_vs_speed/interpolation_type")) {
112  std::string interp_type = config_map.at("curvature_vs_speed/interpolation_type");
113  if (interp_type == "zero_order_hold") {
115  ROS_INFO("Setting interpolation type to %s", interp_type.c_str());
116  } else if (interp_type == "linear") {
118  ROS_INFO("Setting interpolation type to %s", interp_type.c_str());
119  } else {
120  ROS_ERROR("Ignoring invalid interpolation type '%s'.", interp_type.c_str());
121  }
122  config_map.erase("curvature_vs_speed/interpolation_type");
123  }
124 
125  // We read in the curve points by determining if point N exists in the
126  // config set, starting with N = 0 and going until the first pair of points
127  // fails. We don't set the points in the curve because we want to make sure
128  // that a new curve is actually specified before deleting the old curve.
129  std::vector<double> x;
130  std::vector<double> y;
131  for (size_t i = 0; true; i++) {
132  char base_key[1024];
133  snprintf(base_key, sizeof(base_key), "curvature_vs_speed/values/%zu", i);
134  const std::string x_key = std::string(base_key) + "/0";
135  const std::string y_key = std::string(base_key) + "/1";
136 
137  if (config_map.count(x_key) && config_map.count(y_key)) {
138  x.push_back(boost::lexical_cast<double>(config_map.at(x_key)));
139  config_map.erase(x_key);
140  y.push_back(boost::lexical_cast<double>(config_map.at(y_key)));
141  config_map.erase(y_key);
142  } else {
143  break;
144  }
145  }
146 
147  // If a curve was provided, setup speed_curve using the new parameters.
148  // Note that from the way x,y are constructed, they are guaranteed to have
149  // the same length.
150  if (x.size()) {
151  ROS_INFO("Setting curvature_vs_speed curve: ");
153  for (size_t i = 0; i < x.size(); i++) {
154  speed_curve_.appendPoint(x[i], y[i]);
155  ROS_INFO(" %zu -- %lf, %lf", i, x[i], y[i]);
156  }
157  }
158 
159  // Print warnings to help find ignored paramters
160  for (auto const &it : config_map) {
161  ROS_WARN("Ignoring unknown configuration value '%s'", it.first.c_str());
162  }
163 }
164 
165 void SpeedForCurvatureParameters::readToConfig(mcm::KeyValueArray &config) const
166 {
167  mcm::KeyValueArrayPtr msg = boost::make_shared<mcm::KeyValueArray>();
168  config.header.stamp = ros::Time::now();
169 
170  addItem(config, "curvature_filter_size",
171  boost::lexical_cast<std::string>(curvature_filter_size_));
172  addItem(config, "lateral_acceleration_mode",
173  use_speed_from_accel_constant_ ? "1" : "0");
174  addItem(config, "max_lateral_acceleration",
175  boost::lexical_cast<std::string>(max_lateral_accel_mss_));
176  addItem(config, "curvature_vs_speed/interpolation_type",
178 
179  for (size_t i = 0; i < speed_curve_.numPoints(); i++) {
180  char base_key[1024];
181  snprintf(base_key, sizeof(base_key), "curvature_vs_speed/values/%zu", i);
182 
183  addItem(config, std::string(base_key) + "/0",
184  boost::lexical_cast<std::string>(speed_curve_.getPoint(i).first));
185  addItem(config, std::string(base_key) + "/1",
186  boost::lexical_cast<std::string>(speed_curve_.getPoint(i).second));
187  }
188 }
189 
190 static double estimateCurvature(const Route &route,
191  const size_t index,
192  double filter_size)
193 {
194  // Sample the route at the specified point
195  mnm::RoutePosition pos1;
196  pos1.id = route.points[index].id();
197  pos1.distance = 0.0;
198 
199  // Sample the route one filter size backwards.
200  mnm::RoutePosition pos0 = pos1;
201  pos0.distance -= filter_size;
202 
203  // Sample the route one filter size forwards.
204  mnm::RoutePosition pos2 = pos1;
205  pos2.distance += filter_size;
206 
207  // Get the actual route samples
208  RoutePoint pt0;
209  interpolateRoutePosition(pt0, route, pos0, true);
210  RoutePoint pt1;
211  interpolateRoutePosition(pt1, route, pos1, true);
212  RoutePoint pt2;
213  interpolateRoutePosition(pt2, route, pos2, true);
214 
215  // Approximate the incoming tangent vector
216  const tf::Vector3 T0 = (pt1.position() - pt0.position()).normalized();
217  // Approximate the outgoing tangent vector
218  const tf::Vector3 T1 = (pt2.position() - pt1.position()).normalized();
219  // k ~ ||dT / ds||
220  return ((T1-T0)/filter_size).length();
221 }
222 
223 static double maxSpeedForCurvature(double curvature,
224  const SpeedForCurvatureParameters &params)
225 {
226  double k = std::abs(curvature);
227 
228  if (params.use_speed_from_accel_constant_) {
229  double a = std::abs(params.max_lateral_accel_mss_);
230 
231  double max_speed = 1000.0;
232 
233  if (k < 1e-4) {
234  return max_speed;
235  } else {
236  return std::min(max_speed, std::sqrt(a/k));
237  }
238  } else {
239  return params.speed_curve_.eval(k);
240  }
241 }
242 
244  mnm::RouteSpeedArray &speeds,
245  const Route &route,
246  const SpeedForCurvatureParameters &parameters)
247 {
248  speeds.header.stamp = ros::Time::now();
249 
250  // We're going to generate a speed for every route point for now.
251  speeds.speeds.resize(route.points.size());
252 
253  for (size_t i = 0; i < route.points.size(); ++i) {
254  speeds.speeds[i].id = route.points[i].id();
255  speeds.speeds[i].distance = 0.0;
256 
257  double k = estimateCurvature(route, i, parameters.curvature_filter_size_);
258  speeds.speeds[i].speed = maxSpeedForCurvature(k, parameters);
259  }
260 }
261 
263  :
264  origin_to_front_m_(2.0),
265  origin_to_rear_m_(1.0),
266  origin_to_left_m_(1.0),
267  origin_to_right_m_(1.0),
268  max_distance_m_(10.0),
269  min_distance_m_(1.0),
270  max_speed_(10.0),
271  min_speed_(1.0),
272  stop_buffer_m_(5.0)
273 {
274 }
275 
277 {
278  swri::param(pnh, "origin_to_front_m", origin_to_front_m_, 2.0);
279  swri::param(pnh, "origin_to_rear_m", origin_to_rear_m_, 1.0);
280  swri::param(pnh, "origin_to_left_m", origin_to_left_m_, 1.0);
281  swri::param(pnh, "origin_to_right_m", origin_to_right_m_, 1.0);
282 
283  swri::param(pnh, "max_distance_m", max_distance_m_, 10.0);
284  swri::param(pnh, "min_distance_m", min_distance_m_, 1.0);
285  swri::param(pnh, "max_speed", max_speed_, 10.0);
286  swri::param(pnh, "min_speed", min_speed_, 1.0);
287 
288  swri::param(pnh, "stop_buffer_m", stop_buffer_m_, 5.0);
289 }
290 
292  std::vector<ObstacleData> &obstacle_data,
293  const stu::Transform g_route_from_obs,
294  const mnm::ObstacleArray &obstacles_msg)
295 {
296  obstacle_data.resize(obstacles_msg.obstacles.size());
297  for (size_t i = 0; i < obstacle_data.size(); i++) {
298  const mnm::Obstacle &obs_msg = obstacles_msg.obstacles[i];
299 
300  geometry_msgs::Pose pose = obs_msg.pose;
301  if (pose.orientation.x == 0.0 &&
302  pose.orientation.y == 0.0 &&
303  pose.orientation.z == 0.0 &&
304  pose.orientation.w == 0.0) {
305  pose.orientation.w = 1.0;
306  }
307 
308  tf::Transform g_obs_from_local;
309  tf::poseMsgToTF(pose, g_obs_from_local);
310 
311  obstacle_data[i].center = g_route_from_obs*g_obs_from_local.getOrigin();
312  obstacle_data[i].center.setZ(0.0);
313 
314  double max_radius = 0.0;
315  obstacle_data[i].polygon.resize(obs_msg.polygon.size());
316  for (size_t j = 0; j < obs_msg.polygon.size(); j++) {
317  tf::Vector3 pt;
318  tf::pointMsgToTF(obs_msg.polygon[j], pt);
319 
320  max_radius = std::max(max_radius, pt.length());
321  obstacle_data[i].polygon[j] = g_route_from_obs*(g_obs_from_local*pt);
322  obstacle_data[i].polygon[j].setZ(0.0);
323  }
324  obstacle_data[i].radius = max_radius;
325  }
326 }
327 
329  mnm::RouteSpeedArray &speeds,
330  std::vector<DistanceReport> &reports,
331  const Route &route,
332  const mnm::RoutePosition &route_position,
333  const std::vector<ObstacleData> &obstacles,
335 {
336  tf::Vector3 local_fl(p.origin_to_left_m_, p.origin_to_left_m_, 0.0);
337  tf::Vector3 local_fr(p.origin_to_left_m_, -p.origin_to_right_m_, 0.0);
338  tf::Vector3 local_br(-p.origin_to_right_m_, -p.origin_to_right_m_, 0.0);
339  tf::Vector3 local_bl(-p.origin_to_right_m_, p.origin_to_left_m_, 0.0);
340  double car_r = 0.0;
341  car_r = std::max(car_r, local_fl.length());
342  car_r = std::max(car_r, local_fr.length());
343  car_r = std::max(car_r, local_br.length());
344  car_r = std::max(car_r, local_bl.length());
345 
346  bool skip_point = true;
347 
348  for (size_t route_index = 0; route_index < route.points.size(); route_index++) {
349  const RoutePoint &point = route.points[route_index];
350 
351  if (skip_point) {
352  if (point.id() == route_position.id) {
353  skip_point = false;
354  }
355  continue;
356  }
357 
358  // Use half of the vehicle width override as the car radius when smaller
359  // This is used for tight routes that are known to be safe such as when going
360  // though cones. It prevents objects close to the vehicle from causing the vehicle
361  // to stop/slow down too much unnecessarily.
362  double veh_r = car_r;
363  if (point.hasProperty("vehicle_width_override"))
364  {
365  ROS_DEBUG("Speeds for obstacle found vehicle_width_override property");
366  double width = point.getTypedProperty<double>("vehicle_width_override");
367 
368  // Pick the smaller of the radii
369  if (veh_r >= width/2.0)
370  {
371  veh_r = width/2.0;
372  ROS_WARN_THROTTLE(1.0, "Vehicle width being overriden to %0.2f", (float)veh_r);
373  }
374  }
375 
376  for (const auto& obstacle: obstacles) {
377  const tf::Vector3 v = obstacle.center - point.position();
378  const double d = v.length() - veh_r - obstacle.radius;
379  if (d > p.max_distance_m_) {
380  // The obstacle is too far away from this point to be a concern
381  continue;
382  }
383 
384  tf::Vector3 closest_point = obstacle.center;
385 
386  double distance = std::numeric_limits<double>::max();
387  for (size_t i = 1; i < obstacle.polygon.size(); i++) {
389  obstacle.polygon[i - 1],
390  obstacle.polygon[i],
391  point.position()) - veh_r;
392 
393  if (dist < distance) {
394  distance = dist;
396  obstacle.polygon[i - 1],
397  obstacle.polygon[i],
398  point.position());
399  }
400  }
401 
402  if (obstacle.polygon.size() > 1) {
404  obstacle.polygon.back(),
405  obstacle.polygon.front(),
406  point.position()) - veh_r;
407 
408  if (dist < distance) {
409  distance = dist;
411  obstacle.polygon.back(),
412  obstacle.polygon.front(),
413  point.position());
414  }
415  }
416 
417  if (distance > p.max_distance_m_) {
418  // The obstacle is too far away from this point to be a concern
419  continue;
420  }
421 
422  // This obstacle is close enough to be a concern. If the bounding
423  // circles are still not touching, we apply a speed limit based on the
424  // distance.
425  if (distance > 0.0) {
426  DistanceReport report;
427  report.near = false;
428  report.collision = false;
429  report.route_index = route_index;
430  report.vehicle_point = point.position() + (closest_point - point.position()).normalized() * veh_r;
431  report.obstacle_point = closest_point;
432  reports.push_back(report);
433 
434  const double s = std::max(0.0, (distance - p.min_distance_m_) / (
436  double speed = (1.0-s)*p.min_speed_ + s*p.max_speed_;
437 
438  speeds.speeds.emplace_back();
439  speeds.speeds.back().id = point.id();
440  speeds.speeds.back().distance = 0.0;
441  speeds.speeds.back().speed = speed;
442 
443  continue;
444  }
445 
446  DistanceReport report;
447  report.near = false;
448  report.collision = true;
449  report.route_index = route_index;
450  report.vehicle_point = point.position();
451  report.obstacle_point = closest_point;
452  reports.push_back(report);
453 
454  speeds.speeds.emplace_back();
455  speeds.speeds.back().id = point.id();
456  speeds.speeds.back().distance = -p.stop_buffer_m_ - p.origin_to_front_m_;
457  speeds.speeds.back().speed = 0.0;
458 
459  speeds.speeds.emplace_back();
460  speeds.speeds.back().id = point.id();
461  speeds.speeds.back().distance = (-p.stop_buffer_m_ - p.origin_to_front_m_) / 2.0;
462  speeds.speeds.back().speed = 0.0;
463 
464  speeds.speeds.emplace_back();
465  speeds.speeds.back().id = point.id();
466  speeds.speeds.back().distance = 0.0;
467  speeds.speeds.back().speed = 0.0;
468 
469  continue;
470  }
471  }
472 }
473 } // namespace swri_route_util
d
msg
bool appendPoint(double x, double y)
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
std::string interpolationTypeString() const
static void pointMsgToTF(const geometry_msgs::Point &msg_v, Point &bt_v)
T getTypedProperty(const std::string &name) const
void loadFromRosParam(const ros::NodeHandle &pnh)
XmlRpcServer s
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
bool interpolateRoutePosition(RoutePoint &point, const Route &route, const marti_nav_msgs::RoutePosition &position, bool allow_extrapolation)
void setInterpolationType(InterpolationType type)
void speedsForObstacles(marti_nav_msgs::RouteSpeedArray &speeds, std::vector< DistanceReport > &reports, const Route &route, const marti_nav_msgs::RoutePosition &route_position, const std::vector< ObstacleData > &obstacles, const SpeedForObstaclesParameters &parameters)
void readToConfig(marti_common_msgs::KeyValueArray &config) const
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
double eval(double x) const
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
void loadFromRosParam(const ros::NodeHandle &pnh)
static double maxSpeedForCurvature(double curvature, const SpeedForCurvatureParameters &params)
TFSIMD_FORCE_INLINE Vector3 normalized() const
static void param(const ros::NodeHandle &nh, const std::string &name, int &variable, const int default_value)
#define ROS_WARN_THROTTLE(period,...)
#define ROS_INFO(...)
static double estimateCurvature(const Route &route, const size_t index, double filter_size)
tf::Vector3 ProjectToLineSegment(const tf::Vector3 &line_start, const tf::Vector3 &line_end, const tf::Vector3 &point)
std::pair< double, double > getPoint(size_t index) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
bool hasProperty(const std::string &name) const
Definition: route_point.cpp:69
double max_lateral_accel_mss_
Maximum lateral acceleration in accel mode in m/s^2.
Definition: route_speeds.h:48
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
std::vector< RoutePoint > points
Definition: route.h:82
const std::string & id() const
void speedsForCurvature(marti_nav_msgs::RouteSpeedArray &speeds, const Route &route, const SpeedForCurvatureParameters &parameters)
static Time now()
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
double DistanceFromLineSegment(const tf::Vector3 &line_start, const tf::Vector3 &line_end, const tf::Vector3 &point)
void generateObstacleData(std::vector< ObstacleData > &obstacle_data, const swri_transform_util::Transform g_route_from_obs, const marti_nav_msgs::ObstacleArray &obstacles_msg)
const tf::Vector3 & position() const
#define ROS_ERROR(...)
bool readFromParameter(const ros::NodeHandle &node_handle, const std::string &param_name, bool error_if_missing)
void loadFromConfig(const marti_common_msgs::KeyValueArray &config)
TFSIMD_FORCE_INLINE tfScalar length() const
static void addItem(mcm::KeyValueArray &m, const std::string &key, const std::string &value)
swri_math_util::Interpolation1D speed_curve_
Definition: route_speeds.h:52
#define ROS_DEBUG(...)


swri_route_util
Author(s):
autogenerated on Fri Jun 7 2019 22:06:09