route_speeds.h
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 // *****************************************************************************
29 #ifndef SWRI_ROUTE_UTIL_ROUTE_SPEEDS_H_
30 #define SWRI_ROUTE_UTIL_ROUTE_SPEEDS_H_
31 
32 #include <marti_common_msgs/KeyValueArray.h>
33 #include <marti_nav_msgs/ObstacleArray.h>
34 #include <marti_nav_msgs/TrackedObjectArray.h>
35 #include <marti_nav_msgs/RouteSpeedArray.h>
37 #include <swri_route_util/route.h>
39 
40 namespace swri_route_util
41 {
43 {
50 
54 
55  // Filter constant used when estimating route curvature. Larger
56  // values result in smoother curvature estimates with fewer spikes.
58 
59 
61 
62  void loadFromRosParam(const ros::NodeHandle &pnh);
63 
64  void loadFromConfig(const marti_common_msgs::KeyValueArray &config);
65  void readToConfig(marti_common_msgs::KeyValueArray &config) const;
66 };
67 
69  marti_nav_msgs::RouteSpeedArray &speeds,
70  const Route &route,
71  const SpeedForCurvatureParameters &parameters);
72 
73 
75 {
80 
83  double max_speed_;
84  double min_speed_;
85 
87 
89 
90  void loadFromRosParam(const ros::NodeHandle &pnh);
91 };
92 
93 // ObstacleData is an intermediate representation for obstacles to
94 // avoid applying transforms and calculating radii repeatedly.
96 {
97  tf::Vector3 center;
98  double radius;
99 
100  std::vector<tf::Vector3> polygon;
101 };
102 
103 
105 {
106  // True if the bounding circles touch but the actual polygons do not.
107  bool near;
108  // True if the actual polygons touch.
109  bool collision;
110  size_t route_index;
111  tf::Vector3 vehicle_point;
112  tf::Vector3 obstacle_point;
113  double distance;
114 };
115 
116 // Convert an obstacle array message into a ObstacleData by applying a
117 // transform and calculating the radius of each obstacle.
119  std::vector<ObstacleData> &obstacle_data,
120  const swri_transform_util::Transform g_route_from_obs,
121  const marti_nav_msgs::ObstacleArray &obstacles_msg);
122 
124  std::vector<ObstacleData> &obstacle_data,
125  const swri_transform_util::Transform g_route_from_obs,
126  const marti_nav_msgs::TrackedObjectArray &obstacles_msg);
127 
128 void speedsForObstacles(
129  marti_nav_msgs::RouteSpeedArray &speeds,
130  std::vector<DistanceReport> &reports,
131  const Route &route,
132  const marti_nav_msgs::RoutePosition &route_position,
133  const std::vector<ObstacleData> &obstacles,
134  const SpeedForObstaclesParameters &parameters);
135 } // namespace swri_route_util
136 #endif // SWRI_ROUTE_UTIL_ROUTE_SPEEDS_H_
interpolation_1d.h
swri_route_util::SpeedForObstaclesParameters::loadFromRosParam
void loadFromRosParam(const ros::NodeHandle &pnh)
Definition: route_speeds.cpp:276
swri_route_util::SpeedForObstaclesParameters::origin_to_left_m_
double origin_to_left_m_
Definition: route_speeds.h:78
swri_route_util::DistanceReport
Definition: route_speeds.h:104
swri_route_util::SpeedForObstaclesParameters::max_distance_m_
double max_distance_m_
Definition: route_speeds.h:81
swri_route_util::SpeedForCurvatureParameters::max_lateral_accel_mss_
double max_lateral_accel_mss_
Maximum lateral acceleration in accel mode in m/s^2.
Definition: route_speeds.h:49
swri_route_util::SpeedForObstaclesParameters::min_speed_
double min_speed_
Definition: route_speeds.h:84
swri_route_util::DistanceReport::distance
double distance
Definition: route_speeds.h:113
transform.h
swri_route_util::Route
Definition: route.h:57
swri_route_util::SpeedForCurvatureParameters::speed_curve_
swri_math_util::Interpolation1D speed_curve_
Definition: route_speeds.h:53
swri_route_util::ObstacleData
Definition: route_speeds.h:95
swri_route_util::ObstacleData::radius
double radius
Definition: route_speeds.h:98
swri_route_util::speedsForCurvature
void speedsForCurvature(marti_nav_msgs::RouteSpeedArray &speeds, const Route &route, const SpeedForCurvatureParameters &parameters)
swri_route_util::SpeedForObstaclesParameters::min_distance_m_
double min_distance_m_
Definition: route_speeds.h:82
swri_route_util::ObstacleData::center
tf::Vector3 center
Definition: route_speeds.h:97
swri_route_util
Definition: path_util.h:35
swri_route_util::ObstacleData::polygon
std::vector< tf::Vector3 > polygon
Definition: route_speeds.h:100
swri_route_util::SpeedForCurvatureParameters::use_speed_from_accel_constant_
bool use_speed_from_accel_constant_
Definition: route_speeds.h:47
swri_route_util::SpeedForObstaclesParameters::SpeedForObstaclesParameters
SpeedForObstaclesParameters()
Definition: route_speeds.cpp:262
swri_route_util::SpeedForObstaclesParameters
Definition: route_speeds.h:74
swri_route_util::SpeedForObstaclesParameters::stop_buffer_m_
double stop_buffer_m_
Definition: route_speeds.h:86
swri_route_util::SpeedForCurvatureParameters::SpeedForCurvatureParameters
SpeedForCurvatureParameters()
Definition: route_speeds.cpp:52
route.h
swri_route_util::SpeedForCurvatureParameters::loadFromConfig
void loadFromConfig(const marti_common_msgs::KeyValueArray &config)
Definition: route_speeds.cpp:84
swri_route_util::SpeedForObstaclesParameters::max_speed_
double max_speed_
Definition: route_speeds.h:83
swri_route_util::DistanceReport::obstacle_point
tf::Vector3 obstacle_point
Definition: route_speeds.h:112
swri_transform_util::Transform
swri_route_util::SpeedForCurvatureParameters
Definition: route_speeds.h:42
swri_route_util::SpeedForCurvatureParameters::readToConfig
void readToConfig(marti_common_msgs::KeyValueArray &config) const
Definition: route_speeds.cpp:165
swri_route_util::SpeedForObstaclesParameters::origin_to_right_m_
double origin_to_right_m_
Definition: route_speeds.h:79
swri_route_util::SpeedForObstaclesParameters::origin_to_rear_m_
double origin_to_rear_m_
Definition: route_speeds.h:77
swri_route_util::speedsForObstacles
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)
swri_math_util::Interpolation1D
swri_route_util::DistanceReport::collision
bool collision
Definition: route_speeds.h:109
swri_route_util::DistanceReport::near
bool near
Definition: route_speeds.h:107
swri_route_util::SpeedForCurvatureParameters::loadFromRosParam
void loadFromRosParam(const ros::NodeHandle &pnh)
Definition: route_speeds.cpp:60
swri_route_util::SpeedForObstaclesParameters::origin_to_front_m_
double origin_to_front_m_
Definition: route_speeds.h:76
swri_route_util::DistanceReport::vehicle_point
tf::Vector3 vehicle_point
Definition: route_speeds.h:111
swri_route_util::generateObstacleData
void generateObstacleData(std::vector< ObstacleData > &obstacle_data, const swri_transform_util::Transform g_route_from_obs, const marti_nav_msgs::ObstacleArray &obstacles_msg)
swri_route_util::SpeedForCurvatureParameters::curvature_filter_size_
double curvature_filter_size_
Definition: route_speeds.h:57
ros::NodeHandle
swri_route_util::DistanceReport::route_index
size_t route_index
Definition: route_speeds.h:110


swri_route_util
Author(s):
autogenerated on Fri Aug 2 2024 08:39:28