route_speeds.h
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 #ifndef SWRI_ROUTE_UTIL_ROUTE_SPEEDS_H_
00030 #define SWRI_ROUTE_UTIL_ROUTE_SPEEDS_H_
00031 
00032 #include <marti_common_msgs/KeyValueArray.h>
00033 #include <marti_nav_msgs/ObstacleArray.h>
00034 #include <marti_nav_msgs/RouteSpeedArray.h>
00035 #include <swri_math_util/interpolation_1d.h>
00036 #include <swri_route_util/route.h>
00037 #include <swri_transform_util/transform.h>
00038 
00039 namespace swri_route_util
00040 {
00041 struct SpeedForCurvatureParameters
00042 {
00046   bool use_speed_from_accel_constant_;
00048   double max_lateral_accel_mss_;
00049 
00052   swri_math_util::Interpolation1D speed_curve_;
00053 
00054   // Filter constant used when estimating route curvature.  Larger
00055   // values result in smoother curvature estimates with fewer spikes.
00056   double curvature_filter_size_;
00057 
00058 
00059   SpeedForCurvatureParameters();
00060 
00061   void loadFromRosParam(const ros::NodeHandle &pnh);
00062 
00063   void loadFromConfig(const marti_common_msgs::KeyValueArray &config);
00064   void readToConfig(marti_common_msgs::KeyValueArray &config) const;
00065 };
00066 
00067 void speedsForCurvature(
00068   marti_nav_msgs::RouteSpeedArray &speeds,
00069   const Route &route,
00070   const SpeedForCurvatureParameters &parameters);
00071 
00072 
00073 struct SpeedForObstaclesParameters
00074 {
00075   double origin_to_front_m_;
00076   double origin_to_rear_m_;
00077   double origin_to_left_m_;
00078   double origin_to_right_m_;
00079 
00080   double max_distance_m_;
00081   double min_distance_m_;
00082   double max_speed_;
00083   double min_speed_;
00084 
00085   double stop_buffer_m_;
00086 
00087   SpeedForObstaclesParameters();
00088 
00089   void loadFromRosParam(const ros::NodeHandle &pnh);
00090 };
00091 
00092 // ObstacleData is an intermediate representation for obstacles to
00093 // avoid applying transforms and calculating radii repeatedly.
00094 struct ObstacleData
00095 {
00096   tf::Vector3 center;
00097   double radius;
00098 
00099   std::vector<tf::Vector3> polygon;
00100 };
00101 
00102 
00103 struct DistanceReport
00104 {
00105   // True if the bounding circles touch but the actual polygons do not.
00106   bool near;
00107   // True if the actual polygons touch.
00108   bool collision;
00109   size_t route_index;
00110   tf::Vector3 vehicle_point;
00111   tf::Vector3 obstacle_point;
00112   double distance;
00113 };
00114 
00115 // Convert an obstacle array message into a ObstacleData by applying a
00116 // transform and calculating the radius of each obstacle.
00117 void generateObstacleData(
00118   std::vector<ObstacleData> &obstacle_data,
00119   const swri_transform_util::Transform g_route_from_obs,
00120   const marti_nav_msgs::ObstacleArray &obstacles_msg);
00121 
00122 void speedsForObstacles(
00123     marti_nav_msgs::RouteSpeedArray &speeds,
00124     std::vector<DistanceReport> &reports,
00125     const Route &route,
00126     const marti_nav_msgs::RoutePosition &route_position,
00127     const std::vector<ObstacleData> &obstacles,
00128     const SpeedForObstaclesParameters &parameters);
00129 }  // namespace swri_route_util
00130 #endif  // SWRI_ROUTE_UTIL_ROUTE_SPEEDS_H_


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