dwa_planner.h
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2009, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 #ifndef DWA_LOCAL_PLANNER_DWA_PLANNER_H_
38 #define DWA_LOCAL_PLANNER_DWA_PLANNER_H_
39 
40 #include <vector>
41 #include <Eigen/Core>
42 
43 
44 #include <dwa_local_planner/DWAPlannerConfig.h>
45 
46 //for creating a local cost grid
48 
49 //for obstacle data access
50 #include <costmap_2d/costmap_2d.h>
51 
56 
62 
63 #include <nav_msgs/Path.h>
64 
65 namespace dwa_local_planner {
70  class DWAPlanner {
71  public:
78  DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util);
79 
83  void reconfigure(DWAPlannerConfig &cfg);
84 
92  bool checkTrajectory(
93  const Eigen::Vector3f pos,
94  const Eigen::Vector3f vel,
95  const Eigen::Vector3f vel_samples);
96 
105  const geometry_msgs::PoseStamped& global_pose,
106  const geometry_msgs::PoseStamped& global_vel,
107  geometry_msgs::PoseStamped& drive_velocities);
108 
120  void updatePlanAndLocalCosts(const geometry_msgs::PoseStamped& global_pose,
121  const std::vector<geometry_msgs::PoseStamped>& new_plan,
122  const std::vector<geometry_msgs::Point>& footprint_spec);
123 
128  double getSimPeriod() { return sim_period_; }
129 
140  bool getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost);
141 
145  bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
146 
147  private:
148 
150 
151  double stop_time_buffer_;
153  Eigen::Vector3f vsamples_;
154 
155  double sim_period_;
157 
159 
160  std::vector<geometry_msgs::PoseStamped> global_plan_;
161 
162  boost::mutex configuration_mutex_;
163  std::string frame_id_;
165  bool publish_cost_grid_pc_;
166  bool publish_traj_pc_;
167 
168  double cheat_factor_;
169 
171 
172  // see constructor body for explanations
181 
183  };
184 };
185 #endif
dwa_local_planner::DWAPlanner::vsamples_
Eigen::Vector3f vsamples_
Definition: dwa_planner.h:188
ros::Publisher
map_grid_visualizer.h
local_planner_util.h
dwa_local_planner::DWAPlanner::cheat_factor_
double cheat_factor_
Definition: dwa_planner.h:203
base_local_planner::OscillationCostFunction
dwa_local_planner::DWAPlanner::occdist_scale_
double occdist_scale_
Definition: dwa_planner.h:187
base_local_planner::SimpleScoredSamplingPlanner
dwa_local_planner::DWAPlanner::goal_costs_
base_local_planner::MapGridCostFunction goal_costs_
Definition: dwa_planner.h:212
base_local_planner::MapGridVisualizer
base_local_planner::LocalPlannerUtil
costmap_2d.h
dwa_local_planner::DWAPlanner::findBestPath
base_local_planner::Trajectory findBestPath(const geometry_msgs::PoseStamped &global_pose, const geometry_msgs::PoseStamped &global_vel, geometry_msgs::PoseStamped &drive_velocities)
Given the current position and velocity of the robot, find the best trajectory to exectue.
Definition: dwa_planner.cpp:332
dwa_local_planner::DWAPlanner::checkTrajectory
bool checkTrajectory(const Eigen::Vector3f pos, const Eigen::Vector3f vel, const Eigen::Vector3f vel_samples)
Check if a trajectory is legal for a position/velocity pair.
Definition: dwa_planner.cpp:251
simple_scored_sampling_planner.h
obstacle_cost_function.h
dwa_local_planner::DWAPlanner::configuration_mutex_
boost::mutex configuration_mutex_
Definition: dwa_planner.h:197
oscillation_cost_function.h
dwa_local_planner::DWAPlanner::traj_cloud_pub_
ros::Publisher traj_cloud_pub_
Definition: dwa_planner.h:199
dwa_local_planner::DWAPlanner::publish_traj_pc_
bool publish_traj_pc_
Definition: dwa_planner.h:201
dwa_local_planner::DWAPlanner::publish_cost_grid_pc_
bool publish_cost_grid_pc_
Whether or not to build and publish a PointCloud.
Definition: dwa_planner.h:200
dwa_local_planner::DWAPlanner::goal_front_costs_
base_local_planner::MapGridCostFunction goal_front_costs_
Definition: dwa_planner.h:213
dwa_local_planner::DWAPlanner::map_viz_
base_local_planner::MapGridVisualizer map_viz_
The map grid visualizer for outputting the potential field generated by the cost function.
Definition: dwa_planner.h:205
dwa_local_planner::DWAPlanner::twirling_costs_
base_local_planner::TwirlingCostFunction twirling_costs_
Definition: dwa_planner.h:215
dwa_local_planner::DWAPlanner::scored_sampling_planner_
base_local_planner::SimpleScoredSamplingPlanner scored_sampling_planner_
Definition: dwa_planner.h:217
simple_trajectory_generator.h
dwa_local_planner::DWAPlanner::global_plan_
std::vector< geometry_msgs::PoseStamped > global_plan_
Definition: dwa_planner.h:195
dwa_local_planner::DWAPlanner
A class implementing a local planner using the Dynamic Window Approach.
Definition: dwa_planner.h:105
base_local_planner::ObstacleCostFunction
dwa_local_planner::DWAPlanner::getSimPeriod
double getSimPeriod()
Get the period at which the local planner is expected to run.
Definition: dwa_planner.h:163
dwa_local_planner::DWAPlanner::path_distance_bias_
double path_distance_bias_
Definition: dwa_planner.h:187
local_planner_limits.h
map_grid_cost_function.h
dwa_local_planner::DWAPlanner::setPlan
bool setPlan(const std::vector< geometry_msgs::PoseStamped > &orig_global_plan)
Definition: dwa_planner.cpp:242
dwa_local_planner::DWAPlanner::stop_time_buffer_
double stop_time_buffer_
How long before hitting something we're going to enforce that the robot stop.
Definition: dwa_planner.h:186
dwa_local_planner::DWAPlanner::DWAPlanner
DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util)
Constructor for the planner.
Definition: dwa_planner.cpp:153
dwa_local_planner::DWAPlanner::forward_point_distance_
double forward_point_distance_
Definition: dwa_planner.h:193
dwa_local_planner::DWAPlanner::goal_distance_bias_
double goal_distance_bias_
Definition: dwa_planner.h:187
dwa_local_planner::DWAPlanner::path_costs_
base_local_planner::MapGridCostFunction path_costs_
Definition: dwa_planner.h:211
dwa_local_planner::DWAPlanner::generator_
base_local_planner::SimpleTrajectoryGenerator generator_
Definition: dwa_planner.h:208
twirling_cost_function.h
dwa_local_planner::DWAPlanner::updatePlanAndLocalCosts
void updatePlanAndLocalCosts(const geometry_msgs::PoseStamped &global_pose, const std::vector< geometry_msgs::PoseStamped > &new_plan, const std::vector< geometry_msgs::Point > &footprint_spec)
Update the cost functions before planning.
Definition: dwa_planner.cpp:278
dwa_local_planner::DWAPlanner::alignment_costs_
base_local_planner::MapGridCostFunction alignment_costs_
Definition: dwa_planner.h:214
dwa_local_planner::DWAPlanner::planner_util_
base_local_planner::LocalPlannerUtil * planner_util_
Definition: dwa_planner.h:184
base_local_planner::SimpleTrajectoryGenerator
dwa_local_planner
Definition: dwa_planner.h:65
dwa_local_planner::DWAPlanner::sim_period_
double sim_period_
The number of seconds to use to compute max/min vels for dwa.
Definition: dwa_planner.h:190
dwa_local_planner::DWAPlanner::frame_id_
std::string frame_id_
Definition: dwa_planner.h:198
base_local_planner::Trajectory
dwa_local_planner::DWAPlanner::result_traj_
base_local_planner::Trajectory result_traj_
Definition: dwa_planner.h:191
dwa_local_planner::DWAPlanner::obstacle_costs_
base_local_planner::ObstacleCostFunction obstacle_costs_
Definition: dwa_planner.h:210
base_local_planner::MapGridCostFunction
dwa_local_planner::DWAPlanner::reconfigure
void reconfigure(DWAPlannerConfig &cfg)
Reconfigures the trajectory planner.
Definition: dwa_planner.cpp:87
base_local_planner::TwirlingCostFunction
trajectory.h
dwa_local_planner::DWAPlanner::oscillation_costs_
base_local_planner::OscillationCostFunction oscillation_costs_
Definition: dwa_planner.h:209
dwa_local_planner::DWAPlanner::getCellCosts
bool getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)
Compute the components and total cost for a map grid cell.
Definition: dwa_planner.cpp:224


dwa_local_planner
Author(s): Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:33