route_speeds.cpp
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2017, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 #include <swri_route_util/route_speeds.h>
00030 
00031 #include <unordered_map>
00032 
00033 #include <swri_geometry_util/geometry_util.h>
00034 #include <swri_roscpp/swri_roscpp.h>
00035 #include <swri_route_util/util.h>
00036 
00037 namespace smu = swri_math_util;
00038 namespace stu = swri_transform_util;
00039 namespace mcm = marti_common_msgs;
00040 namespace mnm = marti_nav_msgs;
00041 
00042 namespace swri_route_util
00043 {
00044 // Convenience function to add a key/value pair to a KeyValueArray message.
00045 static void addItem(mcm::KeyValueArray &m, const std::string &key, const std::string &value)
00046 {
00047   m.items.emplace_back();
00048   m.items.back().key = key;
00049   m.items.back().value = value;
00050 }
00051 
00052 SpeedForCurvatureParameters::SpeedForCurvatureParameters()
00053   :
00054   use_speed_from_accel_constant_(true),
00055   max_lateral_accel_mss_(0.2),
00056   curvature_filter_size_(20.0)
00057 {
00058 }
00059 
00060 void SpeedForCurvatureParameters::loadFromRosParam(const ros::NodeHandle &pnh)
00061 {
00062   // 20.0 seems to be a good value from looking through 17 recorded routes we
00063   // have with and without Omnistar corrections.
00064   swri::param(pnh, "curvature_filter_size", curvature_filter_size_, 20.0);
00065 
00066 
00067   swri::param(pnh, "lateral_acceleration_mode",
00068               use_speed_from_accel_constant_, use_speed_from_accel_constant_);
00069   swri::param(pnh, "max_lateral_acceleration",
00070               max_lateral_accel_mss_, max_lateral_accel_mss_);
00071 
00072   if (!speed_curve_.readFromParameter(pnh, "curvature_vs_speed", true)) {
00073     ROS_ERROR("Failed to load speed/curve parameter. Forcing lateral acceleration mode.");
00074     use_speed_from_accel_constant_ = true;
00075   } else {
00076     ROS_INFO("Loaded speed vs curvature curve (%s)", speed_curve_.interpolationTypeString().c_str());
00077     for (size_t i = 0; i < speed_curve_.numPoints(); i++) {
00078       std::pair<double, double> pt = speed_curve_.getPoint(i);
00079       ROS_INFO("  %zu -- %f [1/m] vs %f [m/s]", i, pt.first, pt.second);
00080     }
00081   }
00082 }
00083 
00084 void SpeedForCurvatureParameters::loadFromConfig(const mcm::KeyValueArray &config)
00085 {
00086   std::unordered_map<std::string, std::string> config_map;
00087   for (size_t i = 0; i < config.items.size(); ++i) {
00088     config_map[config.items[i].key] = config.items[i].value;
00089   }
00090 
00091   if (config_map.count("curvature_filter_size")) {
00092     curvature_filter_size_ = boost::lexical_cast<double>(config_map.at("curvature_filter_size"));
00093     ROS_INFO("Setting curvature_filter_size to %lf", curvature_filter_size_);
00094     config_map.erase("curvature_filter_size");
00095   }
00096 
00097   if (config_map.count("lateral_acceleration_mode")) {
00098     use_speed_from_accel_constant_ =
00099       boost::lexical_cast<int>(config_map.at("lateral_acceleration_mode"));
00100     ROS_INFO("Setting lateral acceleration mode to %s",
00101              use_speed_from_accel_constant_ ? "true" : "false");
00102     config_map.erase("lateral_acceleration_mode");
00103   }
00104 
00105   if (config_map.count("max_lateral_acceleration")) {
00106     max_lateral_accel_mss_ = boost::lexical_cast<double>(config_map.at("max_lateral_acceleration"));
00107     ROS_INFO("Setting max_lateral_acceleration to %lf", max_lateral_accel_mss_);
00108     config_map.erase("max_lateral_acceleration");
00109   }
00110 
00111   if (config_map.count("curvature_vs_speed/interpolation_type")) {
00112     std::string interp_type = config_map.at("curvature_vs_speed/interpolation_type");
00113     if (interp_type == "zero_order_hold") {
00114       speed_curve_.setInterpolationType(smu::Interpolation1D::ZERO_ORDER_HOLD);
00115       ROS_INFO("Setting interpolation type to %s", interp_type.c_str());
00116     } else if (interp_type == "linear") {
00117       speed_curve_.setInterpolationType(smu::Interpolation1D::LINEAR);
00118       ROS_INFO("Setting interpolation type to %s", interp_type.c_str());
00119     } else {
00120       ROS_ERROR("Ignoring invalid interpolation type '%s'.", interp_type.c_str());
00121     }
00122     config_map.erase("curvature_vs_speed/interpolation_type");
00123   }
00124 
00125   // We read in the curve points by determining if point N exists in the
00126   // config set, starting with N = 0 and going until the first pair of points
00127   // fails.  We don't set the points in the curve because we want to make sure
00128   // that a new curve is actually specified before deleting the old curve.
00129   std::vector<double> x;
00130   std::vector<double> y;
00131   for (size_t i = 0; true; i++) {
00132     char base_key[1024];
00133     snprintf(base_key, sizeof(base_key), "curvature_vs_speed/values/%zu", i);
00134     const std::string x_key = std::string(base_key) + "/0";
00135     const std::string y_key = std::string(base_key) + "/1";
00136 
00137     if (config_map.count(x_key) && config_map.count(y_key)) {
00138       x.push_back(boost::lexical_cast<double>(config_map.at(x_key)));
00139       config_map.erase(x_key);
00140       y.push_back(boost::lexical_cast<double>(config_map.at(y_key)));
00141       config_map.erase(y_key);
00142     } else {
00143       break;
00144     }
00145   }
00146 
00147   // If a curve was provided, setup speed_curve using the new parameters.
00148   // Note that from the way x,y are constructed, they are guaranteed to have
00149   // the same length.
00150   if (x.size()) {
00151     ROS_INFO("Setting curvature_vs_speed curve: ");
00152     speed_curve_.clear();
00153     for (size_t i = 0; i < x.size(); i++) {
00154       speed_curve_.appendPoint(x[i], y[i]);
00155       ROS_INFO("  %zu -- %lf, %lf", i, x[i], y[i]);
00156     }
00157   }
00158 
00159   // Print warnings to help find ignored paramters
00160   for (auto const &it : config_map) {
00161     ROS_WARN("Ignoring unknown configuration value '%s'", it.first.c_str());
00162   }
00163 }
00164 
00165 void SpeedForCurvatureParameters::readToConfig(mcm::KeyValueArray &config) const
00166 {
00167   mcm::KeyValueArrayPtr msg = boost::make_shared<mcm::KeyValueArray>();
00168   config.header.stamp = ros::Time::now();
00169 
00170   addItem(config, "curvature_filter_size",
00171           boost::lexical_cast<std::string>(curvature_filter_size_));
00172   addItem(config, "lateral_acceleration_mode",
00173           use_speed_from_accel_constant_ ? "1" : "0");
00174   addItem(config, "max_lateral_acceleration",
00175           boost::lexical_cast<std::string>(max_lateral_accel_mss_));
00176   addItem(config, "curvature_vs_speed/interpolation_type",
00177           speed_curve_.interpolationTypeString());
00178 
00179   for (size_t i = 0; i < speed_curve_.numPoints(); i++) {
00180     char base_key[1024];
00181     snprintf(base_key, sizeof(base_key), "curvature_vs_speed/values/%zu", i);
00182 
00183     addItem(config, std::string(base_key) + "/0",
00184             boost::lexical_cast<std::string>(speed_curve_.getPoint(i).first));
00185     addItem(config, std::string(base_key) + "/1",
00186             boost::lexical_cast<std::string>(speed_curve_.getPoint(i).second));
00187   }
00188 }
00189 
00190 static double estimateCurvature(const Route &route,
00191                                 const size_t index,
00192                                 double filter_size)
00193 {
00194   // Sample the route at the specified point
00195   mnm::RoutePosition pos1;
00196   pos1.id = route.points[index].id();
00197   pos1.distance = 0.0;
00198 
00199   // Sample the route one filter size backwards.
00200   mnm::RoutePosition pos0 = pos1;
00201   pos0.distance -= filter_size;
00202 
00203   // Sample the route one filter size forwards.
00204   mnm::RoutePosition pos2 = pos1;
00205   pos2.distance += filter_size;
00206 
00207   // Get the actual route samples
00208   RoutePoint pt0;
00209   interpolateRoutePosition(pt0, route, pos0, true);
00210   RoutePoint pt1;
00211   interpolateRoutePosition(pt1, route, pos1, true);
00212   RoutePoint pt2;
00213   interpolateRoutePosition(pt2, route, pos2, true);
00214 
00215   // Approximate the incoming tangent vector
00216   const tf::Vector3 T0 = (pt1.position() - pt0.position()).normalized();
00217   // Approximate the outgoing tangent vector
00218   const tf::Vector3 T1 = (pt2.position() - pt1.position()).normalized();
00219   // k ~ ||dT / ds||
00220   return ((T1-T0)/filter_size).length();
00221 }
00222 
00223 static double maxSpeedForCurvature(double curvature,
00224                                    const SpeedForCurvatureParameters &params)
00225 {
00226   double k = std::abs(curvature);
00227 
00228   if (params.use_speed_from_accel_constant_) {
00229     double a = std::abs(params.max_lateral_accel_mss_);
00230 
00231     double max_speed = 1000.0;
00232 
00233     if (k < 1e-4) {
00234       return max_speed;
00235     } else {
00236       return std::min(max_speed, std::sqrt(a/k));
00237     }
00238   } else {
00239     return params.speed_curve_.eval(k);
00240   }
00241 }
00242 
00243 void speedsForCurvature(
00244   mnm::RouteSpeedArray &speeds,
00245   const Route &route,
00246   const SpeedForCurvatureParameters &parameters)
00247 {
00248   speeds.header.stamp = ros::Time::now();
00249 
00250   // We're going to generate a speed for every route point for now.
00251   speeds.speeds.resize(route.points.size());
00252 
00253   for (size_t i = 0; i < route.points.size(); ++i) {
00254     speeds.speeds[i].id = route.points[i].id();
00255     speeds.speeds[i].distance = 0.0;
00256 
00257     double k = estimateCurvature(route, i, parameters.curvature_filter_size_);
00258     speeds.speeds[i].speed = maxSpeedForCurvature(k, parameters);
00259   }
00260 }
00261 
00262 SpeedForObstaclesParameters::SpeedForObstaclesParameters()
00263   :
00264   origin_to_front_m_(2.0),
00265   origin_to_rear_m_(1.0),
00266   origin_to_left_m_(1.0),
00267   origin_to_right_m_(1.0),
00268   max_distance_m_(10.0),
00269   min_distance_m_(1.0),
00270   max_speed_(10.0),
00271   min_speed_(1.0),
00272   stop_buffer_m_(5.0)
00273 {
00274 }
00275 
00276 void SpeedForObstaclesParameters::loadFromRosParam(const ros::NodeHandle &pnh)
00277 {
00278   swri::param(pnh, "origin_to_front_m", origin_to_front_m_, 2.0);
00279   swri::param(pnh, "origin_to_rear_m", origin_to_rear_m_, 1.0);
00280   swri::param(pnh, "origin_to_left_m", origin_to_left_m_, 1.0);
00281   swri::param(pnh, "origin_to_right_m", origin_to_right_m_, 1.0);
00282 
00283   swri::param(pnh, "max_distance_m", max_distance_m_, 10.0);
00284   swri::param(pnh, "min_distance_m", min_distance_m_, 1.0);
00285   swri::param(pnh, "max_speed", max_speed_, 10.0);
00286   swri::param(pnh, "min_speed", min_speed_, 1.0);
00287 
00288   swri::param(pnh, "stop_buffer_m", stop_buffer_m_, 5.0);
00289 }
00290 
00291 void generateObstacleData(
00292   std::vector<ObstacleData> &obstacle_data,
00293   const stu::Transform g_route_from_obs,
00294   const mnm::ObstacleArray &obstacles_msg)
00295 {
00296   obstacle_data.resize(obstacles_msg.obstacles.size());
00297   for (size_t i = 0; i < obstacle_data.size(); i++) {
00298     const mnm::Obstacle &obs_msg = obstacles_msg.obstacles[i];
00299 
00300     geometry_msgs::Pose pose = obs_msg.pose;
00301     if (pose.orientation.x == 0.0 &&
00302         pose.orientation.y == 0.0 &&
00303         pose.orientation.z == 0.0 &&
00304         pose.orientation.w == 0.0) {
00305       pose.orientation.w = 1.0;
00306     }
00307 
00308     tf::Transform g_obs_from_local;
00309     tf::poseMsgToTF(pose, g_obs_from_local);
00310 
00311     obstacle_data[i].center = g_route_from_obs*g_obs_from_local.getOrigin();
00312     obstacle_data[i].center.setZ(0.0);
00313 
00314     double max_radius = 0.0;
00315     obstacle_data[i].polygon.resize(obs_msg.polygon.size());
00316     for (size_t j = 0; j < obs_msg.polygon.size(); j++) {
00317       tf::Vector3 pt;
00318       tf::pointMsgToTF(obs_msg.polygon[j], pt);
00319       pt = pt - g_obs_from_local.getOrigin();
00320 
00321       max_radius = std::max(max_radius, pt.length());
00322       obstacle_data[i].polygon[j] = g_route_from_obs*(g_obs_from_local*pt);
00323       obstacle_data[i].polygon[j].setZ(0.0);
00324     }
00325     obstacle_data[i].radius = max_radius;
00326   }
00327 }
00328 
00329 void speedsForObstacles(
00330   mnm::RouteSpeedArray &speeds,
00331   std::vector<DistanceReport> &reports,
00332   const Route &route,
00333   const mnm::RoutePosition &route_position,
00334   const std::vector<ObstacleData> &obstacles,
00335   const SpeedForObstaclesParameters &p)
00336 {
00337   tf::Vector3 local_fl(p.origin_to_left_m_, p.origin_to_left_m_, 0.0);
00338   tf::Vector3 local_fr(p.origin_to_left_m_, -p.origin_to_right_m_, 0.0);
00339   tf::Vector3 local_br(-p.origin_to_right_m_, -p.origin_to_right_m_, 0.0);
00340   tf::Vector3 local_bl(-p.origin_to_right_m_, p.origin_to_left_m_, 0.0);
00341   double car_r = 0.0;
00342   car_r = std::max(car_r, local_fl.length());
00343   car_r = std::max(car_r, local_fr.length());
00344   car_r = std::max(car_r, local_br.length());
00345   car_r = std::max(car_r, local_bl.length());
00346 
00347   bool skip_point = true;
00348 
00349   for (size_t route_index = 0; route_index < route.points.size(); route_index++) {
00350     const RoutePoint &point = route.points[route_index];
00351 
00352     if (skip_point) {
00353       if (point.id() == route_position.id) {
00354         skip_point = false;
00355       }
00356       continue;
00357     }
00358     
00359     // Use half of the vehicle width override as the car radius when smaller
00360     // This is used for tight routes that are known to be safe such as when going
00361     // though cones. It prevents objects close to the vehicle from causing the vehicle
00362     // to stop/slow down too much unnecessarily.
00363     double veh_r = car_r;
00364     if (point.hasProperty("vehicle_width_override"))
00365     {
00366       ROS_DEBUG("Speeds for obstacle found vehicle_width_override property");
00367       double width = point.getTypedProperty<double>("vehicle_width_override");
00368 
00369       // Pick the smaller of the radii
00370       if (veh_r >= width/2.0)
00371       {
00372         veh_r = width/2.0;
00373         ROS_WARN_THROTTLE(1.0, "Vehicle width being overriden to %0.2f", (float)veh_r);
00374       }
00375     }
00376 
00377     for (const auto& obstacle: obstacles) {
00378       const tf::Vector3 v = obstacle.center - point.position();
00379       const double d = v.length() - veh_r - obstacle.radius;
00380       if (d > p.max_distance_m_) {
00381         // The obstacle is too far away from this point to be a concern
00382         continue;
00383       }
00384 
00385       tf::Vector3 closest_point = obstacle.center;
00386 
00387       double distance = std::numeric_limits<double>::max();
00388       for (size_t i = 1; i < obstacle.polygon.size(); i++) {
00389         double dist = swri_geometry_util::DistanceFromLineSegment(
00390           obstacle.polygon[i - 1],
00391           obstacle.polygon[i],
00392           point.position()) - veh_r;
00393 
00394         if (dist < distance) {
00395           distance = dist;
00396           closest_point = swri_geometry_util::ProjectToLineSegment(
00397             obstacle.polygon[i - 1],
00398             obstacle.polygon[i],
00399             point.position());
00400         }
00401       }
00402 
00403       if (obstacle.polygon.size() > 1) {
00404         double dist = swri_geometry_util::DistanceFromLineSegment(
00405           obstacle.polygon.back(),
00406           obstacle.polygon.front(),
00407           point.position()) - veh_r;
00408 
00409         if (dist < distance) {
00410           distance = dist;
00411           closest_point = swri_geometry_util::ProjectToLineSegment(
00412             obstacle.polygon.back(),
00413             obstacle.polygon.front(),
00414             point.position());
00415         }
00416       }
00417 
00418       if (distance > p.max_distance_m_) {
00419         // The obstacle is too far away from this point to be a concern
00420         continue;
00421       }
00422 
00423       // This obstacle is close enough to be a concern.  If the bounding
00424       // circles are still not touching, we apply a speed limit based on the
00425       // distance.
00426       if (distance > 0.0) {
00427         DistanceReport report;
00428         report.near = false;
00429         report.collision = false;
00430         report.route_index = route_index;
00431         report.vehicle_point = point.position() + (closest_point - point.position()).normalized() * veh_r;
00432         report.obstacle_point = closest_point;
00433         reports.push_back(report);
00434 
00435         const double s = std::max(0.0, (distance - p.min_distance_m_) / (
00436                                     p.max_distance_m_ - p.min_distance_m_));
00437         double speed = (1.0-s)*p.min_speed_ + s*p.max_speed_;
00438 
00439         speeds.speeds.emplace_back();
00440         speeds.speeds.back().id = point.id();
00441         speeds.speeds.back().distance = 0.0;
00442         speeds.speeds.back().speed = speed;
00443 
00444         continue;
00445       }
00446 
00447       DistanceReport report;
00448       report.near = false;
00449       report.collision = true;
00450       report.route_index = route_index;
00451       report.vehicle_point = point.position();
00452       report.obstacle_point = closest_point;
00453       reports.push_back(report);
00454 
00455       speeds.speeds.emplace_back();
00456       speeds.speeds.back().id = point.id();
00457       speeds.speeds.back().distance = -p.stop_buffer_m_ - p.origin_to_front_m_;
00458       speeds.speeds.back().speed = 0.0;
00459 
00460       speeds.speeds.emplace_back();
00461       speeds.speeds.back().id = point.id();
00462       speeds.speeds.back().distance = (-p.stop_buffer_m_ - p.origin_to_front_m_) / 2.0;
00463       speeds.speeds.back().speed = 0.0;
00464 
00465       speeds.speeds.emplace_back();
00466       speeds.speeds.back().id = point.id();
00467       speeds.speeds.back().distance = 0.0;
00468       speeds.speeds.back().speed = 0.0;
00469 
00470       continue;
00471     }
00472   }
00473 }
00474 }  // namespace swri_route_util


swri_route_util
Author(s):
autogenerated on Tue Oct 3 2017 03:19:53