ROS Wrapper for the DWAPlanner that adheres to the BaseLocalPlanner interface and can be used as a plugin for move_base. More...
#include <dwa_planner_ros.h>
Public Member Functions | |
bool | computeVelocityCommands (geometry_msgs::Twist &cmd_vel) |
Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base. | |
DWAPlannerROS () | |
Constructor for DWAPlannerROS wrapper. | |
void | initialize (std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *costmap_ros) |
Constructs the ros wrapper. | |
bool | isGoalReached () |
Check if the goal pose has been achieved. | |
bool | setPlan (const std::vector< geometry_msgs::PoseStamped > &orig_global_plan) |
Set the plan that the controller is following. | |
Private Member Functions | |
void | odomCallback (const nav_msgs::Odometry::ConstPtr &msg) |
Callback for receiving odometry data. | |
bool | rotateToGoal (const tf::Stamped< tf::Pose > &global_pose, const tf::Stamped< tf::Pose > &robot_vel, double goal_th, geometry_msgs::Twist &cmd_vel) |
Once a goal position is reached... rotate to the goal orientation. | |
double | sign (double x) |
bool | stopWithAccLimits (const tf::Stamped< tf::Pose > &global_pose, const tf::Stamped< tf::Pose > &robot_vel, geometry_msgs::Twist &cmd_vel) |
Stop the robot taking into account acceleration limits. | |
Private Attributes | |
nav_msgs::Odometry | base_odom_ |
costmap_2d::Costmap2DROS * | costmap_ros_ |
boost::shared_ptr< DWAPlanner > | dp_ |
ros::Publisher | g_plan_pub_ |
std::vector < geometry_msgs::PoseStamped > | global_plan_ |
bool | initialized_ |
ros::Publisher | l_plan_pub_ |
bool | latch_xy_goal_tolerance_ |
double | max_vel_th_ |
double | min_rot_vel_ |
double | min_vel_th_ |
boost::mutex | odom_mutex_ |
ros::Subscriber | odom_sub_ |
bool | prune_plan_ |
double | rot_stopped_vel_ |
bool | rotating_to_goal_ |
tf::TransformListener * | tf_ |
double | trans_stopped_vel_ |
double | xy_goal_tolerance_ |
bool | xy_tolerance_latch_ |
double | yaw_goal_tolerance_ |
ROS Wrapper for the DWAPlanner that adheres to the BaseLocalPlanner interface and can be used as a plugin for move_base.
Definition at line 49 of file dwa_planner_ros.h.
dwa_local_planner::DWAPlannerROS::DWAPlannerROS | ( | ) | [inline] |
Constructor for DWAPlannerROS wrapper.
Definition at line 54 of file dwa_planner_ros.h.
bool dwa_local_planner::DWAPlannerROS::computeVelocityCommands | ( | geometry_msgs::Twist & | cmd_vel | ) | [virtual] |
Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base.
cmd_vel | Will be filled with the velocity command to be passed to the robot base |
Implements nav_core::BaseLocalPlanner.
Definition at line 170 of file dwa_planner_ros.cpp.
void dwa_local_planner::DWAPlannerROS::initialize | ( | std::string | name, |
tf::TransformListener * | tf, | ||
costmap_2d::Costmap2DROS * | costmap_ros | ||
) | [virtual] |
Constructs the ros wrapper.
name | The name to give this instance of the trajectory planner |
tf | A pointer to a transform listener |
costmap | The cost map to use for assigning costs to trajectories |
Implements nav_core::BaseLocalPlanner.
Definition at line 46 of file dwa_planner_ros.cpp.
bool dwa_local_planner::DWAPlannerROS::isGoalReached | ( | ) | [virtual] |
Check if the goal pose has been achieved.
Implements nav_core::BaseLocalPlanner.
Definition at line 339 of file dwa_planner_ros.cpp.
void dwa_local_planner::DWAPlannerROS::odomCallback | ( | const nav_msgs::Odometry::ConstPtr & | msg | ) | [private] |
Callback for receiving odometry data.
msg | An Odometry message |
Definition at line 88 of file dwa_planner_ros.cpp.
bool dwa_local_planner::DWAPlannerROS::rotateToGoal | ( | const tf::Stamped< tf::Pose > & | global_pose, |
const tf::Stamped< tf::Pose > & | robot_vel, | ||
double | goal_th, | ||
geometry_msgs::Twist & | cmd_vel | ||
) | [private] |
Once a goal position is reached... rotate to the goal orientation.
global_pose | The pose of the robot in the global frame |
robot_vel | The velocity of the robot |
goal_th | The desired th value for the goal |
cmd_vel | The velocity commands to be filled |
Definition at line 128 of file dwa_planner_ros.cpp.
bool dwa_local_planner::DWAPlannerROS::setPlan | ( | const std::vector< geometry_msgs::PoseStamped > & | orig_global_plan | ) | [virtual] |
Set the plan that the controller is following.
orig_global_plan | The plan to pass to the controller |
Implements nav_core::BaseLocalPlanner.
Definition at line 356 of file dwa_planner_ros.cpp.
double dwa_local_planner::DWAPlannerROS::sign | ( | double | x | ) | [inline, private] |
Definition at line 85 of file dwa_planner_ros.h.
bool dwa_local_planner::DWAPlannerROS::stopWithAccLimits | ( | const tf::Stamped< tf::Pose > & | global_pose, |
const tf::Stamped< tf::Pose > & | robot_vel, | ||
geometry_msgs::Twist & | cmd_vel | ||
) | [private] |
Stop the robot taking into account acceleration limits.
global_pose | The pose of the robot in the global frame |
robot_vel | The velocity of the robot |
cmd_vel | The velocity commands to be filled |
Definition at line 98 of file dwa_planner_ros.cpp.
nav_msgs::Odometry dwa_local_planner::DWAPlannerROS::base_odom_ [private] |
Definition at line 123 of file dwa_planner_ros.h.
Definition at line 113 of file dwa_planner_ros.h.
boost::shared_ptr<DWAPlanner> dwa_local_planner::DWAPlannerROS::dp_ [private] |
Definition at line 124 of file dwa_planner_ros.h.
Definition at line 121 of file dwa_planner_ros.h.
std::vector<geometry_msgs::PoseStamped> dwa_local_planner::DWAPlannerROS::global_plan_ [private] |
Definition at line 125 of file dwa_planner_ros.h.
bool dwa_local_planner::DWAPlannerROS::initialized_ [private] |
Definition at line 119 of file dwa_planner_ros.h.
Definition at line 121 of file dwa_planner_ros.h.
bool dwa_local_planner::DWAPlannerROS::latch_xy_goal_tolerance_ [private] |
Definition at line 127 of file dwa_planner_ros.h.
double dwa_local_planner::DWAPlannerROS::max_vel_th_ [private] |
Definition at line 115 of file dwa_planner_ros.h.
double dwa_local_planner::DWAPlannerROS::min_rot_vel_ [private] |
Definition at line 115 of file dwa_planner_ros.h.
double dwa_local_planner::DWAPlannerROS::min_vel_th_ [private] |
Definition at line 115 of file dwa_planner_ros.h.
boost::mutex dwa_local_planner::DWAPlannerROS::odom_mutex_ [private] |
Definition at line 122 of file dwa_planner_ros.h.
Definition at line 120 of file dwa_planner_ros.h.
bool dwa_local_planner::DWAPlannerROS::prune_plan_ [private] |
Definition at line 118 of file dwa_planner_ros.h.
double dwa_local_planner::DWAPlannerROS::rot_stopped_vel_ [private] |
Definition at line 116 of file dwa_planner_ros.h.
bool dwa_local_planner::DWAPlannerROS::rotating_to_goal_ [private] |
Definition at line 126 of file dwa_planner_ros.h.
Definition at line 114 of file dwa_planner_ros.h.
double dwa_local_planner::DWAPlannerROS::trans_stopped_vel_ [private] |
Definition at line 116 of file dwa_planner_ros.h.
double dwa_local_planner::DWAPlannerROS::xy_goal_tolerance_ [private] |
Definition at line 117 of file dwa_planner_ros.h.
bool dwa_local_planner::DWAPlannerROS::xy_tolerance_latch_ [private] |
Definition at line 127 of file dwa_planner_ros.h.
double dwa_local_planner::DWAPlannerROS::yaw_goal_tolerance_ [private] |
Definition at line 117 of file dwa_planner_ros.h.