dwb_local_planner.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #ifndef DWB_LOCAL_PLANNER_DWB_LOCAL_PLANNER_H
36 #define DWB_LOCAL_PLANNER_DWB_LOCAL_PLANNER_H
37 
43 #include <pluginlib/class_loader.h>
44 #include <memory>
45 #include <string>
46 #include <vector>
47 
48 namespace dwb_local_planner
49 {
50 
56 {
57 public:
62 
63  virtual ~DWBLocalPlanner() {}
64 
71  void initialize(const ros::NodeHandle& parent, const std::string& name,
72  TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
73 
78  void setGoalPose(const nav_2d_msgs::Pose2DStamped& goal_pose) override;
79 
84  void setPlan(const nav_2d_msgs::Path2D& path) override;
85 
98  nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose,
99  const nav_2d_msgs::Twist2D& velocity) override;
100 
111  bool isGoalReached(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) override;
112 
124  virtual dwb_msgs::TrajectoryScore scoreTrajectory(const dwb_msgs::Trajectory2D& traj, double best_score = -1);
125 
138  virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose,
139  const nav_2d_msgs::Twist2D& velocity,
140  std::shared_ptr<dwb_msgs::LocalPlanEvaluation>& results);
141 
142 
143 protected:
149  virtual void prepare(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity);
150 
154  virtual dwb_msgs::TrajectoryScore coreScoringAlgorithm(const geometry_msgs::Pose2D& pose,
155  const nav_2d_msgs::Twist2D velocity,
156  std::shared_ptr<dwb_msgs::LocalPlanEvaluation>& results);
157 
168  virtual nav_2d_msgs::Path2D transformGlobalPlan(const nav_2d_msgs::Pose2DStamped& pose);
169 
173  geometry_msgs::Pose2D transformPoseToLocal(const nav_2d_msgs::Pose2DStamped& pose);
174 
175  nav_2d_msgs::Path2D global_plan_;
176  nav_2d_msgs::Pose2DStamped goal_pose_;
181 
182  // Plugin handling
188  std::vector<TrajectoryCritic::Ptr> critics_;
189 
196  std::string resolveCriticClassName(std::string base_name);
197 
202  virtual void loadCritics(const std::string name);
203 
204  std::vector<std::string> default_critic_namespaces_;
205 
210 
212 };
213 
214 } // namespace dwb_local_planner
215 
216 #endif // DWB_LOCAL_PLANNER_DWB_LOCAL_PLANNER_H
dwb_local_planner::DWBLocalPlanner::goal_checker_
GoalChecker::Ptr goal_checker_
Definition: dwb_local_planner.h:186
dwb_local_planner::DWBLocalPlanner::prune_distance_
double prune_distance_
Definition: dwb_local_planner.h:178
dwb_local_planner::DWBLocalPlanner::global_plan_
nav_2d_msgs::Path2D global_plan_
Saved Global Plan.
Definition: dwb_local_planner.h:175
dwb_local_planner::DWBLocalPlanner::prune_plan_
bool prune_plan_
Definition: dwb_local_planner.h:177
class_loader.h
trajectory_critic.h
nav_core2::LocalPlanner
dwb_local_planner::TrajectoryGenerator::Ptr
std::shared_ptr< dwb_local_planner::TrajectoryGenerator > Ptr
Definition: trajectory_generator.h:65
dwb_local_planner::DWBLocalPlanner::tf_
TFListenerPtr tf_
Definition: dwb_local_planner.h:208
dwb_local_planner::DWBLocalPlanner::transformGlobalPlan
virtual nav_2d_msgs::Path2D transformGlobalPlan(const nav_2d_msgs::Pose2DStamped &pose)
Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses...
Definition: dwb_local_planner.cpp:391
dwb_local_planner::DWBLocalPlanner
Plugin-based flexible local_planner.
Definition: dwb_local_planner.h:55
dwb_local_planner::DWBLocalPlanner::costmap_
nav_core2::Costmap::Ptr costmap_
Definition: dwb_local_planner.h:206
dwb_local_planner::DWBLocalPlanner::loadCritics
virtual void loadCritics(const std::string name)
Load the critic parameters from the namespace.
Definition: dwb_local_planner.cpp:114
dwb_local_planner::DWBLocalPlanner::prepare
virtual void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
Helper method for preparing for the core scoring algorithm.
Definition: dwb_local_planner.cpp:202
goal_checker.h
dwb_local_planner::DWBPublisher
Consolidation of all the publishing logic for the DWB Local Planner.
Definition: publisher.h:60
dwb_local_planner
Definition: backwards_compatibility.h:40
dwb_local_planner::DWBLocalPlanner::goal_checker_loader_
pluginlib::ClassLoader< GoalChecker > goal_checker_loader_
Definition: dwb_local_planner.h:185
dwb_local_planner::DWBLocalPlanner::~DWBLocalPlanner
virtual ~DWBLocalPlanner()
Definition: dwb_local_planner.h:63
dwb_local_planner::DWBLocalPlanner::initialize
void initialize(const ros::NodeHandle &parent, const std::string &name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override
nav_core2 initialization
Definition: dwb_local_planner.cpp:60
dwb_local_planner::DWBLocalPlanner::traj_generator_
TrajectoryGenerator::Ptr traj_generator_
Definition: dwb_local_planner.h:184
dwb_local_planner::DWBLocalPlanner::DWBLocalPlanner
DWBLocalPlanner()
Constructor that brings up pluginlib loaders.
Definition: dwb_local_planner.cpp:53
dwb_local_planner::DWBLocalPlanner::critic_loader_
pluginlib::ClassLoader< TrajectoryCritic > critic_loader_
Definition: dwb_local_planner.h:187
dwb_local_planner::DWBLocalPlanner::critics_
std::vector< TrajectoryCritic::Ptr > critics_
Definition: dwb_local_planner.h:188
dwb_local_planner::DWBLocalPlanner::default_critic_namespaces_
std::vector< std::string > default_critic_namespaces_
Definition: dwb_local_planner.h:204
dwb_local_planner::DWBLocalPlanner::transformPoseToLocal
geometry_msgs::Pose2D transformPoseToLocal(const nav_2d_msgs::Pose2DStamped &pose)
Helper method to transform a given pose to the local costmap frame.
Definition: dwb_local_planner.cpp:477
local_planner.h
dwb_local_planner::DWBLocalPlanner::scoreTrajectory
virtual dwb_msgs::TrajectoryScore scoreTrajectory(const dwb_msgs::Trajectory2D &traj, double best_score=-1)
Score a given command. Can be used for testing.
Definition: dwb_local_planner.cpp:352
dwb_local_planner::DWBLocalPlanner::update_costmap_before_planning_
bool update_costmap_before_planning_
Definition: dwb_local_planner.h:207
dwb_local_planner::DWBLocalPlanner::setPlan
void setPlan(const nav_2d_msgs::Path2D &path) override
nav_core2 setPlan - Sets the global plan
Definition: dwb_local_planner.cpp:174
dwb_local_planner::DWBLocalPlanner::traj_gen_loader_
pluginlib::ClassLoader< TrajectoryGenerator > traj_gen_loader_
Definition: dwb_local_planner.h:183
pluginlib::ClassLoader
dwb_local_planner::DWBLocalPlanner::planner_nh_
ros::NodeHandle planner_nh_
Definition: dwb_local_planner.h:211
publisher.h
dwb_local_planner::DWBLocalPlanner::resolveCriticClassName
std::string resolveCriticClassName(std::string base_name)
try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic"
Definition: dwb_local_planner.cpp:93
nav_core2::Costmap::Ptr
std::shared_ptr< Costmap > Ptr
dwb_local_planner::DWBLocalPlanner::coreScoringAlgorithm
virtual dwb_msgs::TrajectoryScore coreScoringAlgorithm(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D velocity, std::shared_ptr< dwb_msgs::LocalPlanEvaluation > &results)
Iterate through all the twists and find the best one.
Definition: dwb_local_planner.cpp:276
TFListenerPtr
std::shared_ptr< tf2_ros::Buffer > TFListenerPtr
dwb_local_planner::DWBLocalPlanner::goal_pose_
nav_2d_msgs::Pose2DStamped goal_pose_
Saved Goal Pose.
Definition: dwb_local_planner.h:176
dwb_local_planner::DWBLocalPlanner::short_circuit_trajectory_evaluation_
bool short_circuit_trajectory_evaluation_
Definition: dwb_local_planner.h:180
tf
dwb_local_planner::GoalChecker::Ptr
std::shared_ptr< dwb_local_planner::GoalChecker > Ptr
Definition: goal_checker.h:58
dwb_local_planner::DWBLocalPlanner::computeVelocityCommands
nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override
nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
Definition: dwb_local_planner.cpp:180
dwb_local_planner::DWBLocalPlanner::debug_trajectory_details_
bool debug_trajectory_details_
Definition: dwb_local_planner.h:179
dwb_local_planner::DWBLocalPlanner::setGoalPose
void setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose) override
nav_core2 setGoalPose - Sets the global goal pose
Definition: dwb_local_planner.cpp:162
dwb_local_planner::DWBLocalPlanner::isGoalReached
bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override
nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velo...
Definition: dwb_local_planner.cpp:143
ros::NodeHandle
trajectory_generator.h
dwb_local_planner::DWBLocalPlanner::pub_
DWBPublisher pub_
Definition: dwb_local_planner.h:209


dwb_local_planner
Author(s): David V. Lu!!
autogenerated on Sun May 18 2025 02:47:25