dwa_planner_ros.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_ROS_H_
38 #define DWA_LOCAL_PLANNER_DWA_PLANNER_ROS_H_
39 
40 #include <boost/shared_ptr.hpp>
41 #include <boost/thread.hpp>
42 
43 #include <tf/transform_listener.h>
44 
45 #include <dynamic_reconfigure/server.h>
46 #include <dwa_local_planner/DWAPlannerConfig.h>
47 
48 #include <angles/angles.h>
49 
50 #include <nav_msgs/Odometry.h>
51 
55 
57 
59 
60 namespace dwa_local_planner {
67  public:
71  DWAPlannerROS();
72 
79  void initialize(std::string name, tf::TransformListener* tf,
80  costmap_2d::Costmap2DROS* costmap_ros);
81 
86 
93  bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
94 
95 
102  bool dwaComputeVelocityCommands(tf::Stamped<tf::Pose>& global_pose, geometry_msgs::Twist& cmd_vel);
103 
109  bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
110 
115  bool isGoalReached();
116 
117 
118 
119  bool isInitialized() {
120  return initialized_;
121  }
122 
123  private:
127  void reconfigureCB(DWAPlannerConfig &config, uint32_t level);
128 
129  void publishLocalPlan(std::vector<geometry_msgs::PoseStamped>& path);
130 
131  void publishGlobalPlan(std::vector<geometry_msgs::PoseStamped>& path);
132 
134 
135  // for visualisation, publishers of global and local plan
137 
139 
141 
143 
144  dynamic_reconfigure::Server<DWAPlannerConfig> *dsrv_;
145  dwa_local_planner::DWAPlannerConfig default_config_;
146  bool setup_;
148 
150 
151 
153 
154 
156  std::string odom_topic_;
157  };
158 };
159 #endif
boost::shared_ptr< DWAPlanner > dp_
The trajectory controller.
void reconfigureCB(DWAPlannerConfig &config, uint32_t level)
Callback to update the local planner&#39;s parameters based on dynamic reconfigure.
dynamic_reconfigure::Server< DWAPlannerConfig > * dsrv_
void initialize(std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *costmap_ros)
Constructs the ros wrapper.
base_local_planner::OdometryHelperRos odom_helper_
base_local_planner::LatchedStopRotateController latchedStopRotateController_
ROS Wrapper for the DWAPlanner that adheres to the BaseLocalPlanner interface and can be used as a pl...
bool dwaComputeVelocityCommands(tf::Stamped< tf::Pose > &global_pose, geometry_msgs::Twist &cmd_vel)
Given the current position, orientation, and velocity of the robot, compute velocity commands to send...
tf::TransformListener * tf_
Used for transforming point clouds.
costmap_2d::Costmap2DROS * costmap_ros_
bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
Given the current position, orientation, and velocity of the robot, compute velocity commands to send...
bool setPlan(const std::vector< geometry_msgs::PoseStamped > &orig_global_plan)
Set the plan that the controller is following.
dwa_local_planner::DWAPlannerConfig default_config_
void publishLocalPlan(std::vector< geometry_msgs::PoseStamped > &path)
void publishGlobalPlan(std::vector< geometry_msgs::PoseStamped > &path)
~DWAPlannerROS()
Destructor for the wrapper.
DWAPlannerROS()
Constructor for DWAPlannerROS wrapper.
bool isGoalReached()
Check if the goal pose has been achieved.
tf::Stamped< tf::Pose > current_pose_
base_local_planner::LocalPlannerUtil planner_util_


dwa_local_planner
Author(s): Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:59