#include <twist_recovery.h>
Public Member Functions | |
void | initialize (std::string n, tf::TransformListener *tf, costmap_2d::Costmap2DROS *global_costmap, costmap_2d::Costmap2DROS *local_costmap) |
Initialize the parameters of the behavior. | |
void | runBehavior () |
Run the behavior. | |
TwistRecovery () | |
Doesn't do anything: initialize is where the actual work happens. | |
~TwistRecovery () | |
Private Member Functions | |
geometry_msgs::Pose2D | getCurrentLocalPose () const |
double | nonincreasingCostInterval (const geometry_msgs::Pose2D ¤t, const geometry_msgs::Twist &twist) const |
double | normalizedPoseCost (const geometry_msgs::Pose2D &pose) const |
geometry_msgs::Twist | scaleGivenAccelerationLimits (const geometry_msgs::Twist &twist, const double time_remaining) const |
geometry_msgs::Twist | transformTwist (const geometry_msgs::Pose2D &pose) const |
Private Attributes | |
double | angular_acceleration_limit_ |
double | angular_speed_limit_ |
geometry_msgs::Twist | base_frame_twist_ |
double | controller_frequency_ |
costmap_2d::Costmap2D | costmap_ |
double | duration_ |
costmap_2d::Costmap2DROS * | global_costmap_ |
bool | initialized_ |
double | linear_acceleration_limit_ |
double | linear_speed_limit_ |
costmap_2d::Costmap2DROS * | local_costmap_ |
std::string | name_ |
ros::NodeHandle | nh_ |
ros::Publisher | pub_ |
double | simulation_inc_ |
tf::TransformListener * | tf_ |
base_local_planner::CostmapModel * | world_model_ |
Recovery behavior that takes a given twist and tries to execute it for up to d seconds, or until reaching an obstacle.
Definition at line 50 of file twist_recovery.h.
twist_recovery::TwistRecovery::TwistRecovery | ( | ) |
Doesn't do anything: initialize is where the actual work happens.
Definition at line 55 of file twist_recovery.cpp.
twist_recovery::TwistRecovery::~TwistRecovery | ( | ) |
Definition at line 59 of file twist_recovery.cpp.
gm::Pose2D twist_recovery::TwistRecovery::getCurrentLocalPose | ( | ) | const [private] |
Definition at line 181 of file twist_recovery.cpp.
void twist_recovery::TwistRecovery::initialize | ( | std::string | n, | |
tf::TransformListener * | tf, | |||
costmap_2d::Costmap2DROS * | global_costmap, | |||
costmap_2d::Costmap2DROS * | local_costmap | |||
) |
Initialize the parameters of the behavior.
double twist_recovery::TwistRecovery::nonincreasingCostInterval | ( | const geometry_msgs::Pose2D & | current, | |
const geometry_msgs::Twist & | twist | |||
) | const [private] |
double twist_recovery::TwistRecovery::normalizedPoseCost | ( | const geometry_msgs::Pose2D & | pose | ) | const [private] |
void twist_recovery::TwistRecovery::runBehavior | ( | ) |
Run the behavior.
Definition at line 192 of file twist_recovery.cpp.
geometry_msgs::Twist twist_recovery::TwistRecovery::scaleGivenAccelerationLimits | ( | const geometry_msgs::Twist & | twist, | |
const double | time_remaining | |||
) | const [private] |
geometry_msgs::Twist twist_recovery::TwistRecovery::transformTwist | ( | const geometry_msgs::Pose2D & | pose | ) | const [private] |
double twist_recovery::TwistRecovery::angular_acceleration_limit_ [private] |
Definition at line 89 of file twist_recovery.h.
double twist_recovery::TwistRecovery::angular_speed_limit_ [private] |
Definition at line 87 of file twist_recovery.h.
geometry_msgs::Twist twist_recovery::TwistRecovery::base_frame_twist_ [private] |
Definition at line 83 of file twist_recovery.h.
double twist_recovery::TwistRecovery::controller_frequency_ [private] |
Definition at line 90 of file twist_recovery.h.
costmap_2d::Costmap2D twist_recovery::TwistRecovery::costmap_ [private] |
Definition at line 73 of file twist_recovery.h.
double twist_recovery::TwistRecovery::duration_ [private] |
Definition at line 85 of file twist_recovery.h.
costmap_2d::Costmap2DROS* twist_recovery::TwistRecovery::global_costmap_ [private] |
Definition at line 71 of file twist_recovery.h.
bool twist_recovery::TwistRecovery::initialized_ [private] |
Definition at line 77 of file twist_recovery.h.
double twist_recovery::TwistRecovery::linear_acceleration_limit_ [private] |
Definition at line 88 of file twist_recovery.h.
double twist_recovery::TwistRecovery::linear_speed_limit_ [private] |
Definition at line 86 of file twist_recovery.h.
costmap_2d::Costmap2DROS* twist_recovery::TwistRecovery::local_costmap_ [private] |
Definition at line 72 of file twist_recovery.h.
std::string twist_recovery::TwistRecovery::name_ [private] |
Definition at line 74 of file twist_recovery.h.
ros::NodeHandle twist_recovery::TwistRecovery::nh_ [private] |
Definition at line 70 of file twist_recovery.h.
ros::Publisher twist_recovery::TwistRecovery::pub_ [private] |
Definition at line 76 of file twist_recovery.h.
double twist_recovery::TwistRecovery::simulation_inc_ [private] |
Definition at line 91 of file twist_recovery.h.
tf::TransformListener* twist_recovery::TwistRecovery::tf_ [private] |
Definition at line 75 of file twist_recovery.h.
base_local_planner::CostmapModel* twist_recovery::TwistRecovery::world_model_ [mutable, private] |
Definition at line 81 of file twist_recovery.h.